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ABSTRACT 


i 


There  are  several  common  approaches  used  to  obtain  the 
kinematic  and  dynamic  equations  which  describe  the  motion  of 
robot  manipulators.  However,  a  problem  arises  when  these 
conventional  body  oriented  robot  arm  kinematic  equations  are 
used  to  simulate  the  manipulator  motion.  In  this  case,  the 

3  <v- 

■jocobian  matrix  which  relates  the  end  effector  motion  to 
joint  angle  variations  becomes  singular  when  two  successive 
arm  links  align.  When  the  robot  arm  passes  through  these 
singular  points,  the  jacobian  matrix  is  not  invertible,  and  *  - 
a  result  of  this,  the  motion  cannot  be  simulated.  This 
thesis  investigates  how  this  situation  can  be  avoided  by 
using  a  Newton  Euler  approach  to  variable  difinition,  and 
using  the  equations  interpretted  in  a  fixed  reference  frame. 


3 


TABLE  OF  CONTENTS 


I.  INTRODUCTION  .  25 

II.  THEORETICAL  DEVELOPMENT  .  28 

A.  THEORY  OF  THE  SOLUTION  .  28 

B.  DYNAMIC  EQUATIONS  OF  MOTION  FOR  LINK  ONE  .  30 

1 .  Force  Equations  .  30 

2.  Joint  Equations  .  31 

3.  Moment  Equations  .  32 

C.  DYNAMIC  EQUATIONS  OF  MOTION  FOR  LINK  TWO  .  36 

1 .  Force  Equations  .  36 

2.  Joint  Equations  .  37 

3.  Moment  Equations  .  39 

D.  DYNAMIC  EQUATIONS  OF  MOTION  FOR  LINK  THREE  .  41 

1.  Force  Equations  .  41 

2.  Joint  Equations  .  41 

3.  Moment  Equations  .  43 

III.  COMPUTATIONAL  APPROACH  .  45 

A.  PROGRAM  MATRICIES  . .  45 

B.  CONSTRAINTS  IN  THE  SIMULATION  PROGRAM  .  50 

C.  PROGRAM  VALIDATION  .  52 

IV.  RESULTS  AND  RECOMMENDATIONS  . ' .  61 

APPENDIX  A:  DERIVATION  OF  THE  TRANSFORMATION 

MATRIX  .  63 

APPENDIX  B:  DIRECT  DYNAMICS  SIMULATION  PROGRAM  1  .  67 


4 


APPENDIX  C:  DIRECT  DYNAMICS  SIMULATION  PROGRAM  2  .  83 

LIST  OF  REFERENCES  . 101 

INITIAL  DISTRIBUTION  LIST  . 104 


5 


LIST  OF  FIGURES 


1.  Free  Body  Diagram  of  a  Three  Link  Manipulator  .  29 

2.  Computer  Simulation  Flow  Chart  .  46 

3.  Matrix  Entries  .  47 

4.  Moment  of  Inertia  .  51 

5.  Validation  Procedure  Flow  Chart  .  54 

6.  Initial  Orientation  of  Links  for  Validation  ........  55 

7.  Configuration  A,  Link  2  Wx  Motion  .  57 

8.  Configuration  A,  Link  2  Wz  Motion  ...  .  58 

9.  Percent  Error  Between  Theoretical  an*. 

Simulated  Angles  .  59 

10.  Critical  Angles  .  66 


TABLE  OF  THE  SYMBOLS  AND  ABBREVIATIONS 


COMPUTER 

SYMBOL 

TEXT 

VARIABLE 

DESCRIPTION 

A 

A 

Sine  Wave  Input  Torque 

Amplitude 

AA 

Aa 

Acceleration  of  Point  A 

AB 

Ab 

Acceleration  of  Point  B 

AG1 

Agl 

The  Acceleration  Vector 

of  the  Center  of  Gravity 

for  Link  1 

AG2 

Ag2 

Same  as  AG1  but  for  Link 

2 

AG3 

Ag3 

Same  as  AG1  but  for  Link 

3 

AOX 

aox 

Linear  Acceleration  of 

Joint  Zero  in  the  X 

Direction 

AOY 

aoy 

Linear  Acceleration  of 

Joint  Zero  in  the  Y 

Direction 

AOZ 

aoz 

Linear  Acceleration  of 

Joint  Zero  in  the  Z 
Direction 


7 


AX1  axl  Linear  Acceleration  of 

Link  One  in  the  X 
Di rection 

AYl  ayl  Linear  Acceleration  of 

Link  One  in  the  Y 
Direction 

AZ1  azl  Linear  Acceleration  of 

Link  One  in  the  Z 
Direction 

AX2  ax2  Linear  Acceleration  of 

Link  Two  in  the  X 
Direction 

AY2  ay2  Linear  Acceleration  of 

Link  Two  in  the  Y 
Direction 

AZ2  az2  Linear  Acceleration  of 

Link  Two  in  the  Z 
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Direction  Cosine  Angle 
in  Degrees  in  Fixed 
Coordinate  System  from  Y 
Axis  fdr  Link  1-3 


DRCANZ(l) 
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DRCRAX ( 2 ) 
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Direction  Cosine  Angle 
in  Radians  in  Fixed 
Coordinate  System  from 
Z  Axis  for  Link  1-3 
Respectively 

The  Argument  of  Direction 
Cosine  Angle  in  the  x 
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DRCSZ(l) 

The  Argument  of  Directioi 

DRCSZ( 2) 

Cosine  Angle  in  the  z 

DRCSZ(3) 

Direction  for  Link  1-3 

Respectively 

DQ( 27 ) 

DQ 
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Obtained  by  Multiplying 
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Fxo 
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Computed  Force  in  the  Z 
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Computed  Force  in  the  Y 

Direction  at  Joint  2 
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Computed  Force  in  the  Z 

Direction  at  Joint  2 
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HDX( 2 ) 


HDx 


HDY ( 2 )  HDy 

HDZ ( 2 )  HDz 

I 

IA 

IER 

IDGT 

IXX( 3,2)  Ixx 

! 

IYY (3,2)  Iyy 

IZZ ( 3 , 2 )  I zz 

l 


The  Time  Rate  of  Change 
of  Angular  Momentum  of  a 
Two  Elements  Composite 
Body  in  the  X  Direction 
Same  as  HDX  but  in  the  Y 
Direction 

Same  as  HDX  but  in  the  Z 

Direction 

Counter 

Row  Dimension  of  Matrix 
A  and  Matrix  B  Used  in 
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Error  Parameter  Used  in 
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Same  as  IXX  but  About 
the  Z  Axis 
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IXZ(3,2)  Ixz 


IXY (3,2) 

Ixy 

IYZ(3,2) 

Iyz 

IXXT( 3 ) 

IYYT(3 ) 

IZZT( 3 ) 

IXZT( 3 ) 

IXYT( 3 ) 

IYZT( 3 ) 

JXO 

jxo 

JYO 

jyo 

A  3*2  Matrix  of  Products 
of  Inertia  for  the  Two 
Element  Composite  Body 
of  Link  1-3  About  the  XZ 
Coordinate  Axis 
Same  as  IXZ  but  for  the 
XY  Axis 

Same  as  IXZ  but  for  the 
YZ  Axis 

Total  Moment  of  Inertia 
of  Link  1-3  About  X  Axis 
Same  as  IXXT  but  About 
the  Y  Axis 

Same  as  IXXT  but  About 
the  Z  Axis 

Same  as  IXXT  but  About 
the  XZ  Axis 

Same  as  IXXT  but  About 
the  XY  Axis 

Same  as  IXXT  but  About 
the  YZ  Axis 

Location  of  Joint  Zero 
in  the  X  Direction 
Location  of  Joint  Zero 
in  the  Y  Direction 


13 


JZO 

jzo 

JX1 

jxl 

JY1 

jyi 

JZ1 

jzl 

JX2 

jx2 

JY2 

jy2 

JZ2 

jz2 

L(  3 , 2 ) 

L( 3 , 2 

LCOGX ( 3 ) 

LCOGx 

LCOGY ( 3 ) 

LCOGy 

LCOGZ( 3 ) 

LCOGz 

Location  of  Joint  Zero 
in  the  Z  Direction 
Location  of  Joint  One  in 
the  X  Direction 
Location  of  Joint  One  in 
the  Y  Direction 
Location  of  Joint  One  in 
the  Z  Direction 
Location  of  Joint  Two  in 
the  X  Direction 
Location  of  Joint  Two  in 
the  Y  Direction 
Location  of  Joint  Two  in 
the  Z  Direction 
A  3*2  Matrix  that  is  the 
Distance  from  Center  of 
Link  to  Center  of  Mass  at 
Each  Link  End 
A  1*3  Location  of  Center 
of  Gravity  Vector  for 
Link  1-3  in  the  X 
Direction 

Same  as  LCOGX  but  for 
the  Y  Direction 
Same  as  LCOGX  but  for 
the  Z  Direction 
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M 


i 


MASS(3,2)  Mass(3,2) 


MASS1 

Ml 

MASS2 

M2 

MASS  3 

M3 

MATA(27 , 27 ) 

MatA 

MATB(27 ) 

MatB 

MATC(27 )  Mate 


MAT1R,  Matlr 

MAT2R,  Mat2r 


Number  of  the  Right  Hand 
Side  Used  in  LEQT2F 
A  3*2  Matrix  of  Mass  of 
Each  Element  that  Make 
Up  the  Composite  Body 
for  Link  1-3 
Total  Mass  of  Link  1 
Total  Mass  of  Link  2 
Total  Mass  of  Link  3 
A  27*27  Matrix  Consisting 
of  Coefficients  of  the 
Unknown  Variables 
A  27*1  Vector  Consisting 
of  the  Coefficient  of 
Known  Variables  on  Input 
and  an  Output  the  Solut¬ 
ion  to  the  Linear  Equat¬ 
ions 

A  27*1  Column  Matrix 
which  Contains  the 
Elements  of  the  Known 
MatB  in  Simulation 
Process , 

Transformation  Matricies 
from  Fixed  Coordinate 
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t 

I 

MAT3R 


MAT IT, 
MAT2T, 
MAT3T 


MI 

MJ 

MK 

MI AO, MJ AO 
and  MKAO 

MIBO.MJBO 
and  MKBO 


Mat3r 


Mat  It 
Mat2t 
Mat3t 


System  to  Body  Fixed 
(Rotating)  Coordinate 
System  for  Link  1  Link  2 
and  Link  3  Respectively 
Transformation  Matricies 
from  Body  Fixed 
Coordinate  System  "to  Yaw 
Pitch  and  Roll  'Angles 
Coordinate  System  for 
Link  1  Link  2  and  Link  3 
Respectively 
Results  From  Subroutine 
CPROD,  I  Component  of 
Vector  Cross  Product 
J  Component  of  Vector 
Cross  Product 
K  Component  of  Vector 
Cross  Product 
Cross  Product  Between 
WDl  and  RB/G1  at  Joint 
Zero  Link  One,  in  X,  Y,  Z 
Direction 

Cross  Product  Between 
W1  and  RB/G1  at  Joint 
Zero  Link  One,  in  X,  Y,  Z 
Direction 


16 


MICO.MJCO 
and  MKCO 

MIA1.MJA1 
and  MKA1 

MIB1.MJB1 
and  MKB1 

MIC1.MJC1 
and  MKC1 

MIA2 , MJA2 
and  MKA2 

MIB2 , MJB2 
and  MKB2 


Cross  Product  Between 
Wl  and  MIBO,  MJBO,  MKBO 
at  Joint  Zero  Link  One, 
in  X,  Y,  Z  Direction 
Cross  Product  Between 
WD1  and  RA/G1  at  Joint 
One  Link  One,  in  X,  Y,  Z 
Direction 

Cross  Product  Between  Wl 
and  RA/G1  at  Joint  One 
Link  One,  in  X,  Y,  Z 
Direction 

. Cross  Product  Between  Wl 
and  MIB1 ,  MJB1,  MKB1  at 
Joint  One  Link  One,  in  X, 
Y,  Z  Direction 
Cross  Product  Between 
WD2  and  RB/G2  at  Joint 
One  Link  Two,  in  X,  Y,  Z 
Direction 

Cross  Product  Between  W2 
and  RB/G1  at  Joint  One 
Link  Two,  in  X,  Y,  Z 
Direction 
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MIC2.MJC2 
and  MKC2 


MI A3, MJ A3 
and  MKA3 


MIB3 , MJB3 
and  MKB3 


MIC3.MJC3 
and  MKC3 


MIA4 , MJA4 
and  MKA4 


MIB4.MJB4 
and  MKB4 


Cross  Product  Between  W2 
and  MIB2,  MJB2,  MKB2  at 
Joint  One  Link  Two,  in  X, 
Y,  Z  Direction 
Cross  Product  Between 
WD2  and  RA/G2  at  Joint 
Two  Link  Two,  in  X,  Y,  Z 
Direction 

Cross  Product  Between  W2 
and  RA/G2  at  Joint  Two 
Link  Two,  in  X,  Y,  Z 
Direction 

Cross  Product  Between  W2 
and  MIB3,  MJB3 ,  MKB3  at 
Joint  Two  Link  Two,  in  X, 
Y,  Z  Direction 
Cross  Product  Between 
WD3  and  RA/G3  at  Joint 
Two  Link  Three,  in  X,  Y, 
Z  Direction 

Cross  Product  Between  W3 
and  RA/G3  at  Joint  Two 
Link  Three,  in  X,  Y,  Z 
Direction 
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MIC4.MJC4 
and  MKC4 


N 

P 

PTRY(l) 

PTRY ( 2 ) 

PTRY ( 3 ) 

PTCANY ( 1 ) 

PTCANY ( 2 ) 

PTCANY ( 3 ) 

RADEG 

RATE1(3)  Ratel 


RATE2(3)  Rate 2 

RATE3(3 )  Rate 3 


Cross  Product  Between  W3 
and  MIB4,  MJB4,  MKB4  at 
Joint  Two  Link  Three,  in 
X,  Y,  Z  Direction 
Order  of  MATA  and  #  of 
Rows  in  MATB 
Phase  Angle  of  Sine  Wave 
Pitch  Angle  in  Radians 
for  Link  1-3  Respectively 

Pitch  Angle  in  Degrees 
for  Link  1-3  Respectively 

Conversion  from  Radians 
to  Degrees 

Angular  Velocity  Vector 
in  Yaw  Pitch  and  Roll 
Coordinate  System  for 
Link  1  in  the  x,  y  and  z 
Direction  Repectively 
Same  as  Ratel  but  for 
the  Link  2 

Same  as  Ratel  but  for 
the  Link  3 


* 
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RX(3,2)  Rx( 3, 2) 


RY ( 3 , 2 ) 

RY ( 3 , 2 ) 

RZ( 3 , 2 ) 

*  Rz(3 , 2 ) 

RAG1 ( 3 ) 

ra/Gl 

RBG1 (3) 

rb/Gl 

RAG2 ( 3 ) 

ra/G2 

RBG2( 3 ) 

rb/G2 

A  3*2  Matrix  Consisting 
of  the  Distance  from  the 
COG  of  the  Link  to  Center 
of  for  Elements  of  Link 
1-3  in  the  X  Direction 
Same  as  RX(3,2)  but  in 
the  Y  Direction 
Same  as  RX(3,2)  but  in 
the  Z  Direction 
A  1*3  Vector,  Distance 
of  Point  A  to  COG  for 
Link  One,  in  X,  Y,  Z 
Direction 

A  1*3  Vector,  Distance 
of  Point  B  to  COG  for 
Link  One,  X,  Y,  Z  Direct¬ 
ion 

A  1*3  Vector,  Distance  of 
Point  A  to  COG  for  Link 
Two,  in  X,  Y,  Z  Direction 
A  1*3  Vector,  Distance  of 
Point  B  to  COG  for  Link 
Two,  i'n  X,  Y,  Z  Direction 
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RBG3(3) 

rb/G3 

A  1*3  Vector,  Distance  of 

Point  B  to  COG  for  Link 

Three,  in  X,  Y,  Z 

Direction 

RLRZ(l) 

Roll  Angle  in  Radians  for 

RLRZ ( 2 ) 

RLRZ ( 3 ) 

Link  1-3  Respectively 

ROLANZ(l) 

Roll  Angle  in  Degrees  for 

ROLANZ(2) 

ROLANZ( 3 ) 

Link  1-3  Respectively 

SUMHDX( 3 ) 

HDx 

A  1*3  Vector,  Sum  of  the 

HDX  for  the  Two  Elements 

of  Link  1-3  in  the  X 

Direction 

SUMHDY ( 3 ) 

HDy 

Same  as  HDX  but  in  the  Y 

Direction 

SUMHDZ ( 3 ) 

HDz 

Same  as  HDX  but  in  the  Z 

Direction 

TOX, TOY, 

Tox, Toy 

Input  Torque  at  Joint  0 

TOZ 

Toz 

in  X,  Y,  Z  Direction 

T1X.T1Y 

Tlx.Tly 

Input  Torque  at  Joint  One 

T1Z 

Tlz 

in  X,  Y.,  Z  Direction 

T2X.T2Y 

T2x, T2y 

Input  Torque  at  Joint  Two 

T2Z 

T2z 

in  X,  Y,  Z  Direction 

TIPX.TIPY 

Position  of  the  End 
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TIP2 

Effector 

VECTAO ( 3 ) 

1*3  Vector  Used  in  Subro¬ 

VECTBO ( 3 ) 

utine  CPROD  for  Joint 

Zero 

VECTA1 ( 3 ) 

1*3  Vector  Used  in  Subro¬ 

VECTB1 ( 3 ) 

utine  CPROD  for  Joint  One 

VECTA2 ( 3 ) 

1*3  Vector  Used  in  Subro¬ 

VECTB2 ( 3 ) 

utine  CPROD  for  Joint  Two 

VECTAO) 

1*3  Vector  Used  in  Subro¬ 

VECTBO) 

utine  CPROD 

W 

w 

Frequency  of  Sine  Input 

WG1.WG2 

wgl , wg2 

Weight  of  Links  1,  2,  3 

WG3 

wg3 

Wl(3 ) 

wl(3) 

A  1*3  Vector  of  the 

Angular  Velocity  of  Link 

1  in  x,  y,  and  z 

Direction  Respectively 

W2(  3 ) 

w2(3) 

Same  as  Wl(3)  but  for 

the  Link  2 

W3(3) 

w3(  3 ) 

Same  as  Wl(3)  but  for 

the  Link  3 

WDX( 3 ) 

wdx ( 3 ) 

Angular  Acceleration  of 

Link  1-3  in  the  X 

Direction 

22 


WDY(3) 


wdy ( 3 ) 


Angular  Acceleration  of 
Link  1-3  in  the  Y 


WDZ ( 3 ) 

WKAREA 

X1,X2,X3 

Y1.Y2.Y3 


YWXR(l) 
YWXR( 2 ) 
YWXR( 3 ) 
YAWANX(l) 
YAWANX( 2) 
YAWANX ( 3 ) 
Z1.Z2.Z3 


Direction 

wdz(3)  Angular  Acceleration  of 

Link  1-3  in  the  Z 
Direction 

Work  Area  for  the  LEQT2F 
Location  of  the  COG  for 
Link  1-3  in  the  X 
Direction 

Location  of  the  COG  for 
Link  1-3  in  the  Y 
Direction 

Yaw  Angle  in  Radians  for 
Link  1-3  Respectively 

Yaw  Angle  in  Degrees  for 
Link  1-3  Respectively 

Location  of  the  COG  for 
Link  1-3  in  the  Z 
Direction 
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I. 


The  study  of  robotics  is  a  fairly  new  discipline. 
Although  the  roots  of  these  studies  and  developments  can  be 
traced  back  to  the  1940’s,  the  first  commercial  computer 
controlled  robot  was  not  introduced  until  the  late  1950 ’s 
[Ref.  1].  Furthermore,  as  the  theory  developed,  several 
common  problemmatical  methods  have  been  widely  accepted  and 
used. 

When  robot  motion  is  studied,  it  is  usually  divided 
into  two  parts:  robot  arm  dynamics  and  robot  arm  kinematics. 
While  the  kinematics  problem  deals  with  the  geometry  of  the 
arm  links,  the  dynamics  problem  deals  with  the  study  of 
forced  motion.  The  dynamics  problem  is  further  divided  into 
two  parts:  the  direct  dynamics  problem  and  the  inverse 
dynamics  problem.  In  the  inverse  dynamics  problem,  link 
variables  such  as  acceleration  and  velocity  are  known  and 
the  forces  and  necessary  joint  torques  for  the  desired 
motion  are  calculated.  In  the  direct  dynamics  problem,  the 
joint  torques  are  known  and  the  accelerations  and  velocities 
of  each  joint  are  calculated. 

The  kinematics  problem  is  also  divided  into  two  parts: 
the  direct  kinematics  problem  and  the  inverse  kinematics 
problem.  The  direct  kinematics  problem  is,  given  a  set  of 
critical  geometric  joint  and  link  variables  for  each  of  the 
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joint-link  pairs  and  the  joint  angle  vector,  determine  the 
position  and  orientation  of  the  end  effector  of  the 
manipulator.  The  Denavit-Hartenberg  representation,  which 
uses  a  homogeneous  transformation  matrix  to  describe  the 
spatial  relationships  between  two  adjacent  rigid  mechanical 
links  is  the  most  common  method  used  to  study  the  direct 
kinematics  problem  [Ref.  2].  The  inverse  kinematics  problem 
is,  given  a  desired  position  and  orientation  of  the  end 
effector  of  the  manipulator  and  a  set  of  critical  geometric 
joint  and  link  parameters,  determine  the  corresponding  joint 
angle  vector;  i.e.,  find  all  of  the  joint  angles  of  the 
robot  arm  so  that  the  end  effector  can  be  positioned  in  the 
desired  location. 

A  difficulty  in  the  solution  to  the  inverse  kinematics 
problem  arises  when  two  successive  links  align  [Ref.  3].  At 
these  times  the  angle  between  two  successive  links  becomes  0 
or  180  degrees,  and  the  Jacobian  matrix  which  relates  the 
end  effector  motion  to  the  joint  variable  variations  cannot 
be  inverted.  This  means  that  motion  cannot  be  simulated. 
Different  approaches  to  this  problem  have  been  investigated. 
One  method  deals  with  the  Newton-Euler  approach  with  a 
moving  coordinate  system  [Refs.  4,  5],  another  uses  the 
Langrangian  approach  [Refs.  6,  7],  One  method  deals  with  a 
virtual  work  approach  [Ref.  8].  Kane’s  dynamics  equations 
have  been  used  due  to  computational  efficiency  [Ref.  9]. 
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However,  none  of  these  methods  have  been  able  to  overcome 
this  singularity  problem  [Ref.  3]. 

Several  methods  have  been  proposed  to  avoid  the 
singular  configuration.  One  method  proposed  to  minimize  the 
time  near  the  singular  points  [Ref.  10],  thereby  reducing 
their  effects.  In  another  method,  it  was  proposed  to  avoid 
these  singular  points  by  confining  the  motion  [Ref.  11]. 
Other  solutions  deal  with  presenting  equations  that  can 
translate  the  manipulator  in  the  neighborhood  of  a 
singularity  through  identification  of  singular  points 
beforehand  [Refs.  12,  13,  14].  It  has  also  been  shown  that 
the  redundancy  of  robot  manipulators  is  effective  in  dealing 
with  the  singularities  [Refs.  15,  16,  17]. 

In  this  thesis  the  equations  of  motion  are  derived 
using  the  principles  of  Newtonian  dynamics  in  terms  of  a 
globally  fixed  coordinate  system  to  overcome  the  singularity 
problem.  Each  link  is  treated  as  a  free  body  with  forces 
and  moments  applied  at  the  joints  and  free  body  analysis  is 
used  to  derive  the  equations  of  motion.  Although  the 
equations  are  relatively  long  and  the  solution  to  the 
problem  is  computationally  time  consuming,  it  is  shown  that 
these  equations  do  overcome  the  singularity  problem.  The 
direct  dynamics  and  the  inverse  dynamics  problem  are  both 


simulated . 


II.  THEORETICAL  DEVELOPMENT 


A.  THEORY  OF  THE  SOLUTION 

To  derive  the  non-singular  equations  of  motion  the 
Newton-Euler  approach  is  used  (Figure  1).  Each  link  is 
treated  as  a  free  body  with  forces  and  moments  applied  to 
it,  weight  has  been  disregarded.  The  globally  fixed  X  Y  Z 
coordinate  system  is  used  for  the  equations.  All  links  are 
assumed  to  be  rigid,  so  the  effects  of  flexibility  are  not 
considered.  All  of  the  distances  and  the  directions  of  the 
forces  and  moments  have  been  based  on  the  fixed  coordinate 
system  rather  than  a  local  coordinate  system  which  moves 
with  the  link  [Refs.  4,  5].  The  link  masses,  the  initial 
link  positions  and  the  orientations  are  assumed  to  be  known 
parameters.  As  a  result  of  equation  derivation  in  the  fixed 
reference  frame,  the  moment  of  inertia  is  allowed  to  change 
with  respect  to  time  and  is  calculated  for  each  small 
Integration  interval.  This  is  opposed  to  keeping  inertia 
constant  as  used  in  the  local  coordinate  formulations.  But 
it  is  assumed  that  the  moment  of  inertia  is  constant  in  each 
small  integration  interval.  This  last  assumption 
effectively  linearizes  the  equations  of  motion  so  that  a 
non-singular  matrix  inversion  can  be  used  to  solve  the 
equation  set. 
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To  calculate  the  moment  of  inertia  in  each  integration 
interval,  the  link  direction  cosine  angles  with  respect  to 
the  fixed  coordinate  system  were  used.  Acceleration  of 
joint  zero  was  input  as  zero.  For  each  link  the  three 
linear  acceleration  components,  three  angular  acceleration 
components  and  forces  at  each  joint  were  considered  to  be 
the  unknown  variables .  Based  on  the  Newtonian  dynamics  and 
the  manipulator  kinematics  [Ref.  18],  the  equations  were 
derived  as  follows: 


B.  DYNAMIC  EQUATIONS  OF  MOTION  OF  LINK  ONE 


In  the  free  body  analysis  of  link  one  (Figure  1)  the 


sum  of  the  forces  in  the  x  direction  is: 


2Fx  =  Fxl  -  FxO  =  Mlaxl  (1) 

Similarly  sum  of  the  forces  in  the  y  direction  is: 

ZFy  =  Fyl  -  FyO  =  Mlayl  (2) 

and  the  sum  of  the  forces  in  the  z  direction  is: 

ZFz  =  Fzl  -  FzO  -  W1  =  Mlazl  (3) 
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2. 


We  begin  by  evaluating  the  joint  equations  at  joint 
zero  [Ref.  19,  equation  (8/4),  pp.  423].  If  the  joint  is 
sequested  and  analysis  conducted  at  a  point  on  link  zero 
(subscript  a)  and  another  at  a  point  on  link  one  (subscript 
b)  that  is  common  to  both,  so  when  linked  together  they  are 
equal.  This  results  in  two  equations  and  the  two  unknowns 
wdl  and  wl .  As  a  result: 

Aa  =  Ao 

which  is  the  acceleration  at  joint  zero,  and 

Ab  =  A1  +  (wdl  x  rb/Gl)  +  wl  x  (wl  x  rb/Gl) 

which  is  the  acceleration  of  point  b  on  joint  one.  Here 
rb/Gl  is  the  distance  from  point  b  to  the  center  of  gravity 
of  link  one,  and  A1  is  the  acceleration  at  the  center  of 
mass  of  link  one  or, 

rb/Gl  =  ( jxO-LCOGxl)i  +  ( jyO-LCOGyl ) j  +  ( jzO-LCOGzl )k 
=  rb/Glx  +  rb/Gly  +  rb/Gl z 

After  equating  Aa  and  Ab  and  having  the  known  variables  on 
the  right  side  of  the  equation  and  unknown  variables  on  the 
left  side  the  following  sets  of  equations  result: 
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Axl  +  wdyl(rb/Glz)  -  wdzl(rb/Gly)  =  Aox  -  MICO 


(4) 


where  MICO  equals 

=  wylwxl ( rb/Gly )  -  ( wyl ) 2 ( rb/Glx)  -  (w2l )2(rb/Glx) 
+  wzlwxl( rb/Glz) 

also 


Ayl  +  wdzl( rb/Glx)  -  wdxl(rb/Glz)  =  Aoy  -  MJCO  (5) 

where  MJCO  equals 

=  wzlwyl (rb/Glz)  -  (wzl )2( rb/Gly )  -  (wxl )2( rb/Gly) 

+  wxlwyl( rb/Glx) 

and 


Azl  +  wdxl( rb/Gly)  -  wdyl( rb/Glx)  =  Aoz  -  MKCO  (6) 
MKCO  equals 

=  wxlwzl (rb/Glx)  -  (wzl ) 2( rb/Glz )  -  ( wyl ) 2 ( rb/Glz ) 

+  wylwzl( rb/Gly) 

3.  Sum  of  Moment  Equations 

Computing  the  sum  of  the  moment  equations  about  the 
center  of  gravity  results  in: 
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ZM1  =  (rO/Gl  x  FO)  +  (rl/Gl  x  FI)  -  T1  +  TO 


where  the  vector  rO/Gl  is  the  distance  from  joint  zero  to 
the  center  of  gravity  of  link  one  and  vector  rl/Gl  is  the 
distance  from’  joint  one  to  the  center  of  gravity  of  link 
one,  in  the  x,  y  and  z  directions.  Such  that 
rO/Gl  =  rjO  -  rGl 

and 

rl/Gl  =  rjl  -  rGl 


so 

rjO  -  rGl  =  (xjO  -  xGl)i  +  (yjO  -  yGl)j  +  (zjO  -  zGl)k 

and 

rjl  -  rGl  =  (xjl  -  xGl ) i  +  (yjl  -  yGl)j  +  (zjl  -  zGl)k 
In  the  x,  y  and  z  directions  the  sum  of  moment  equations 
are: 

2M1  in  x  direction  = 

(yjO/Gl )FzO  +  (z jO/Gl )FyO  +  (yjl/Gl)Fzl  -  (zjl/Gl)Fyl 
-  Tlx  +  TOx  (7a) 

XMl  in  y  direction= 

( z  jO/Gl  )FxO  +  (x jO/Gl  )FzO  +  (zjl/Gl)'Fxl  -  (xjl/Gl)Fzl 
-Tly  +  TOy  (8a) 
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ZM1  in  z  direction: 

(xjO/Gl )FyO  ♦  (yjO/Gl )FxO  +  (xjl/Gl)Fyl  -  (yjl/Gl)Fxl 
-Tlz  +  TOz  (9a) 

From  [Ref. 19,  equation  (57),  pp.  227]  the  sum  of  the  moments 
about  a  fixed  point  that  does  not  move  with  the  body  is 
equal  to  the  time  rate  of  change  of  angular  momentum  of  the 
system  (H)  about  the  fixed  point,  2M  =  H.  In  the  present 
study  we  have  let  each  link  be  a  composite  body  of  two 
elements.  The  angular  momentum  (H)  for  a  composite  body 
where  the  number  of  elements  of  the  body  is  two,  about  the 
center  of  gravity  of  each  link  is  Hi  =  2  [Ri  x  (w  x 

Ri)]Mi,  where  Ri  is  the  distance  from  the  center  of  gravity 
of  each  link  to  the  appropriate  element  (1  or  2)  in  the  x,  y 
and  z  direction.  So: 

Hx  =  2  [Ryi(wx(Ryi)  -  wy(Rxi))  -  Rzi(wz(Rxi)~ 

wx(Rzi ) ) ]Mi 

Hx  =  [R2yl(wx)  -  Ryl(Rxl)(wy)  -  Rzi(Rxl)(wz)  + 
R2  zl ( wx) ]M1  +  [R2y2(wx)  -  Ry2(Rx2 ) ( wy ) - 

Rz2(Rx2)(wz)  +  (R2z2)wx]M2 

If  Ixx  =  Ry2  +  Rz2  dm, 
and  Ixy  =  RxRy  dm, 
and  Ixz  =  RxRz  dm, 
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then: 


Hx  =  [Ilxx(wx)  -  Ilxy(wy)  -  Ilxz(wz)]Ml 

+  CI2xx(wx)  -  I2xy(wy)  -  I2xz(wz)]M2 

and 

HDx  =  [Ilxx(wdx)  -  Ilxy(wdy)  -  Ilxz(wdz)]Ml 

+  [I2xx( wdx)  -  I2xy(wdy)  -  I2xz(wdz) ]M2  (7b) 

by  assuming  the  moment  of  inertia  changes  with  time  but  is 
constant  for  a  given  time  interval. 

By  similar  analysis  it  can  be  shown: 

Hy  =  2  [Rzi(wy(Rzi)  -  wz(Ryi))  -  Rxi(wx(Ryi)- 

wy(Rxi)  -  wy (Rxi ) ) ]Mi 
and  if  Iyy  =  Rx2  +  Rz2  dm, 
and  Iyz  =  RyRz  dm, 

and  Ixy  =  RxRy  dm, 

then: 

HDy  =  [Ilyy(wdy)  -  Ilyz(wdz)  -  Ilyz(wdx)]Ml 

+  [I2yy(wdy)  -  I2yz(wdz)  -  I2yx(wdx)]M2  (8b) 

and 

Hz  =  2  [  Rxi(wz(Rxi)  -  wx(Rzi))  -  Ryi(wy(Rzi)- 

wz(Ryi) ) ]Mi 

if  Izz  =  Rx2  +  Ry2  dm, 

So  Hz  =  [Ilzz(wz)  -  Ilyz(wy)  -  Ilzx(wx)jMl 

+  [I2zz(wz)  -  I2yz(wy)  -  I2zx(wx)]M2 
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then 

HDz  =  [Ilzz(wdz)  -  Ilyz(wdy)  -  Ilzx(wdx)]Ml 

+  [I2zz(wdz)  -  I2yz(wdy)  -  I2zx(wdx)]M2  (9b) 

Combining  equations  (7a)  and  (7b)  and  keeping  known 
variables  on  the  right  side  and  unknown  variables  on  the 
left  side  yields: 

ZMlx  =  ( -y jO/Gl )FzO  +  (zjO/Gl)FyO  +  (yjl/Gl)Fzl 

-  (zjl/Gl)Fyl  -  HDx  =  Tlx  -  TOx  (7) 


Combining  equations  (8a)  and  (8b)  yields: 


ZMly  =  ( -z jO/Gl )FxO  +  (xjO/Gl)FzO  +  (zjl/Gl)Fxl 

-  (x jl/Gl )Fzl  -  HDy  =  Tly  -  TOy  (8) 

Combining  equations  (9a)  and  (9b)  yields: 

ZMlz  =  - (xjO/Gl )FyO  +  (yjO/Gl)FxO  +  (xjl/Gl)Fyl 

-  (yjl/Gl )Fxl  -  HDz  =  Tlz  -  TOz  (9) 

C.  LINK  TWO  EQUATIONS 

1.  Sum  of  Forces  Equations 

From  the  free  body  diagram  (Figure  1)  it  follows 

that 
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ZFx  =  Fx2 

-  Fxl  =  M2ax2 

(10) 

2Fy  =  Fy2 

-  Fyl  =  M2ay2 

(ID 

2Fz  =  Fz2 

-  Fzl  =  M2az2 

(12) 

2.  Joint  Equations 

Analysis  is  conducted  at  joint  one  where  similar 
equations  are  used  as  in  joint  zero  with  a  point  on  link  one 
(a)  and  one  on  link  two  (b).  For  point  a  the  equation  is 

Aa  =  A1  +  wdl  x  ra/Gl  +  wl  x  (wl  x  ra/Gl ) 

ra/Gl  is  a  vector  whose  distance  is  measured  from  point  a  to 
the  center  of  gravity  of  link  one  in  the  x,  y  and  z 
direction . 

ra/Gl  =  (jxl  -  LC0Gxl)i  +  (jyl  -  LC0Gyl)j 
+  (jzl  -  LC0Gzl)k 
=  ra/Glx  +  ra/Gl y  +  ra/Glz 

For  point  b  the  equation  is: 

Ab  =  A2  +  wd2  x  rb/G2  +  w2  x  (w2  x  rb/G2 ) 
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where  rb/G2  is  a  vector  whose  distance  is  measured  from 
point  b  to  the  center  of  gravity  of  link  two. 

rb/G2  =  (jxl  -  LC0Gx2 ) i  +  (jyl  -  LC0Gy2)j 
+  (jzl  -  LC0Gz2)k 
=  rb/G2x  +  rb/G2y  +  rb/G2z 

Equating  Aa  and  Ab  and  setting  knowns  and  unknowns 
on  the  respective  sides  of  the  equation  results  in: 

Ax2  -  Axl  +  wdy2(rb/G2z)  -  wdz2(rb/G2y)  -  wdyl(ra/Glz) 

+  wdzl ( ra/Gly )  =  MIC1  -  M1C2  (13) 

MIC1  =  wylwxl( ra/Gly)  -  ( wyl ) 2 ' ra/Glx)  -  (wzl )2(ra/Glx) 
+  wzlwxl (ra/Glz) 

MIC2  =  wy2wx2(rb/G2y)  -  (wy2)2(rb/G2x)  -  (wz2)2(rb/G2x) 
+  wz2wx2(rb/G2z) 

Ay2  -  Ayl  +  wdz2(rb/G2x  -  wdx2(rb/G2z)  -  wdzl (ra/Glx) 

+  wdxl (ra/Glz)  =  MJC1  -  MJC2  (14) 

MJC1  =  wslwyl (ra/Glz)  -  (wzl )2(ra/Gly)  -  (wxl )2( ra/Gly) 
+  wxlwyl (ra/Glx) 
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MJC2  =  wz2wy2(rb/G2z)  -  (mz2 ) 2 ( rb/G2y )  -  (wx2)2(rb/G2y) 
+  wx2wy2(rb/G2x) 

AZ2  -  AZ1  +  wdx2(rb/G2y)  -  wdy2(rb/G2x)  -  wdxl(ra/Gly) 

+  wdyl (ra/Glx)  =  MKC1  -  MKC2  (15) 

MKC1  =  wxlwzl (ra/Glx)  -  ( wxl ) 2 ( ra/Glz )  -  ( wyl ) 2 ( ra/Glz ) 
+  wylwzl (ra/Gly) 

MKC2  =  wx2wz2(rb/G2x)  -  ( wx2 ) 2 ( rb/G2z )  -  ( wy2 ) 2 ( rb/G2z ) 
+  wy2wz2(rb/G2y) 

3.  Sum  of  the  Moment  Equations 

These  equations  have  a  similar  development  as  that 
of  link  one: 


where 


ZM2  = 

( r  jl/G2 ) 

x  FI  +  ( 

r  j2/G2) 

x  F2  +  T1  -  T2 

rjl/G2 

=  (xjl 

-  xG2)i  + 

(yji  - 

yG2 ) j  +  (zjl  - 

zG2)k 

rj2/G2 

=  ( xj2 

-  xG2)i  + 

( y  J  2  - 

yG2 ) j  +  (zj2  - 

zG2)k 

2M2x  = 

-  (yji 

-  yG2)Fzl 

+  (zjl 

-  zG2 )Fyl 

+  (yj2 

-  yG2)Fz2 

-  (zj2 

-  zG2 )Fy2 

+  Tlx  - 

T2x 

(16a) 
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2M2 y  =  -  (zjl  -  zG2 )Fxl  +  (xjl  -  xG2)Fzl 

+  (zj2  -  zG2)Fx2  -  (xj2  -  xG2)Fz2 

+  Tly  -  T2y  (17a) 

2M2z  =  -  (xjl  -  xG2)Fyl  +  (yjl  -  yG2)Fxl 

+  (xj2  -  xG2 )Fy2  (yj2  -  yG2)Fx2 

+  Tlz  -  T2z  (18a) 

From  angular  momentum  equation  developed  for  link  one,  it 
can  be  shown  for  link  two: 


2M2x  =  HDx 

(16b) 

2M2y  =  HDy 

(17b) 

2M2z  =  HDz 

(18b) 

Combining  equations 

(16a)  and  (16b) 

the  following 

result : 

-  (yjl  -  yG2)Fzl  +  (zjl 

-  zG2 )Fyl  + 

(yj2 

-  yG2 )Fz2 

-  (zj2  -  zG2 )Fy2  -  HDx 

=  -  Tlx  +  T2x 

(16) 

Combining  equations 

(17a)  and 

(17b)  yield  the 

following  result: 

-  (zjl  -  zG2)Fxl  +  (xjl 

-  xG2 )Fzl  + 

(zj2 

-  zG2 )Fx2 

-  (xj2  -  xG2 )Fz2  -  HDy 

=  -Tly  +  T2y 

(17) 
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Combining  equations  (18a)  and  (18b)  yield  the 
following  result: 

-  (xjl  -  xG2)Fyl  +  (yjl  -  yG2)Fxl  +  (xj2  -  xG2)Fy2 

-  (yj 2  -  yG2 )Fx2  -  HDz  =  -Tlz  +  T2z  (18) 

D.  LINK  THREE  EQUATIONS 

1.  Sum  of  Forces  Equations 

Following  similar  logic  from  that  previously 

developed: 

2Fx  =  -  Fx2  =  M3ax3 

ZFy  =  -  Fy2  =  M3ay3 
2Fz  =  -  Fz2  -  W3  =  M3az3 

2 .  Joint  Equations 
With  point  a  on  link  two  and  point  b  on  link  three 

one  gets  for  joint  equations  at  joint  two: 

Aa  =  A2  +  (wd2  x  ra/G2)  +  w2  x  (w2  x  ra/G2) 

where  ra/G2  is  a  vector  whose  distance  is  measured  from 
point  a  to  center  of  gravity  of  link  two  in  the  x,  y  and  z 
direction . 


(19) 

(20) 
(21) 
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ra/G2  =  (jx2  -  LC0Gx2)i  +  (jy2  -  LC0Gy2)j 
+  (jz2  -  LC0Gz2)k 
•  =  ra/G2x  +  ra/G2y  +  ra/G2z 


For  point  b 

Ab  =  A3  +  wd3  x  rb/G3  +  w3  x  (w3  x  rb/G3) 

where  rb/G3  is  a  vector  whose  distance  is  measured  from 
point  b  to  center  of  gravity  of  link  three  in  the  x,  y  and  z 
direction. 

rb/G3  =  (jx2  -  LC0Gx3)i  +  (jy2  -  LC0Gy3)j 
+  (jz2  -  LC0Gz3)k 
=  rb/G3x  +  rb/G3y  +  rb/G3z 

Equating  Aa  and  Ab  and  setting  knowns  and  unknowns  on  the 
respective  sides  of  the  equation  results  in: 

Ax3  -  Ax2  +  wdy3(rb/G3z)  -  wdz3(rb/G3y)  -  wdy2(ra/G2z) 

+  wdz2( ra/G2y )  =  MIC3  -  MIC4  (22) 

MIC3  =  wy2wx2( ra/G2y)  -  (wy2)2(ra/G2x)  -  ( wz2 ) 2 ( ra/G2x) 
+  wz2wx2( ra/G2z ) 

MIC4  =  wy3wx3(rb/G3y)  -  (wy3)2(rb/G3x)  -  ( wz3 ) 2 ( rb/G3x 


+  wz3wx3(rb/G3z) 


Ay3  -  Ay2  +  wdz3(rb/G3x)  -  wdx3(rb/G3z)  -  wdz2(ra/G2x) 


+  wdx2(ra/G2z)  =  MJC3  -  MJC4  (23) 

MJC3  =  wz2wy2(ra/G2z)  -  (wz2)2(ra/G2y)  -  (wx2 ) 2( ra/G2y ) 
+  wx2wy2(ra/G2x) 

MJC4  =  wz3wy3(rb/G3z)  -  w2z3(rb/G3y)  -  w2x3(rb/G3y) 

+  wx3wy3 ( rb/G3x) 

AZ3  -  AZ2  +  wdx3(rb/G3y)  -  wdy3(rb/G3x)  -  wdx2(ra/G2y) 

+  wdy2 ( ra/G2x)  =  MKC3  -  MKC4  (24) 

MKC3  =  wx2wz2 ( ra/G2x)  -  (wx2 ) 2 ( ra/G2z )  -  (wy2 ) 2 ( ra/G2z ) 
+  wx2wy 2 ( ra/G2y ) 

MKC4  =  wx3wz3(rb/G3x)  -  ( wx3 ) 2( rb/G3z )  -  ( wy3 ) 2( rb/G3z ) 
+  wy3wz3 ( rb/G3y ) 

3.  Sim  .of  ..Moment  .Eaoations 

As  in  the  development  of  the  equations  for  link  one: 

ZM3  =  ( r j2/G3 )  x  F2  +  T2 


where 


r J2/G3  =  (xj2  -  xG3)i  +  (yj2  -  yG3)j  +  (zj2  -  zG3)k 
=  x J2/G3  +  yj2/G3  +  zj2/G3 


ZM3x  =  ( -yj2/G3 )Fz2  +  (zj2/G3)Fy2  +  T2x  (25a) 

ZM3y  =  ( -z J2/G3 )Fx2  +  (xj2/G3)Fz2  +  T2y  (26a) 

2M3z  =  ( -x j2/G3 )Fy2  +  (yj2/G3)Fx2  +  T2z  (27a) 

From  the  angular  momentum  theory: 

ZM3x  =  HDx  (25b) 

2M3y  =  HDy  (26b) 

2M3z  =  HDz  (27b) 

Combining  equations  (25a)  and  (25b)  the  following  results: 

(~yj2/G3)Fz2  +  (zj2/G3)Fy2  -  HDx  =  -  T2x  (25) 

Combining  equations  (26a)  and  (26b)  the  following  results: 

( -z J2/G3 )Fx2  +  (xj2/G3)Fz2  -  HDy  =  -  T2y  (26) 

Combining  equations  (27a)  and  (27b)  the  following  results: 

( -xj2/G3 )Fy2  +  (yj2/G3)Fx2  -  HDz  =  -  T2z  (27) 
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III. 


A.  PROGRAM  MATRICIES 

The  Dynamic  Simulation  Language  (DSL)  was  used  to 
simulate  the  motion.  This  computer  code  was  compiled  on  an 
IBM  3033  computer  by  using  the  FORTVS  compiler  and  all  of 
the  calculations  have  been  done  in  double  precision.  The 
entire  simulation  process  is  shown  in  Figure  2  and  is 
discussed  below. 

The  principle  program  matrix,  Matrix  A  (MATA,  27*27), 
was  created  from  the  coefficients  of  the  unknown  variables 
in  equations  1  to  27.  In  the  simulation  of  the  direct 
dynamics  problem,  a  corresponding  27*1  Matrix  B  (MatB)  was 
generated  from  all  known  variables,  also  from  equations  1  to 
27.  A  subroutine  CPROD  was  used  to  perform  all  the  cross 
product  terms  required  in  the  main  program.  The  resulting 
equations  are  shown  in  Figure  3,  in  the  final  matrix  form. 
During  a  simulation  time  step,  the  link  inertias,  the  link 
velocities  and  the  link  positions  were  all  assumed  constant. 
IMSL  subroutine  LEQT2F  was  called  to  invert  the  matrix  A  and 
get  the  generalized  solution  x  from  Ax  =  B.  This  subroutine 
uses  Gaussian  elimination  with  iterative  improvement  to  get 
a  high  accuracy  solution  to  the  problem.  The  output  from 
LEQT2F  then  returns  as  MATB,  which  contains  the  solution  to 
the  equations.  The  outputs  were  used  by  DSL  to  integrate 
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Figure  3.  Matrix  Entries 
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the  linear  and  angular  accelerations  of  each  link  to  get  the 
linear  and  angular  velocities  respectively.  The  linear 
velocities  for  each  link  were  next  integrated  to  get  the 
linear  displacements  of  center  of  gravity  of  each  link. 
Although  the  linear  velocities  in  the  fixed  reference  frame 
can  be  integrated  to  get  the  linear  displacements,  this  idea 
is  not  true  for  the  angular  displacements  [Refs.  20,  21]. 

To  get  the  angular  displacements,  a  set  of 
transformation  matrices  must  be  used  on  the  velocities,  then 
the  motion  can  be  integrated.  That  is,  the  angular 
velocities  of  each  link  in  the  fixed  reference  system  must 
first  be  converted  into  t  ailavences  in  a  body  fixed 
coordinate  system  then  into  body  Euler  rates  and  Euler 
angles  to  define  the  motion  unambigously .  In  this  thesis, 
the  body  coordinate  velocities  are  called  Bratel,  Brate2  and 
Brate3  for  the  link  one,  link  two  and  link  three 
respectively.  To  convert  these  velocities  into  the  Euler 
rates,  another  transformation  matrix  is  used.  That  is,  the 
transformation  matrix  is  multiplied  by  body  rates  to  get  the 
Euler  angle  rates  for  each  link.  These  later  rates  are 
defined  as  the  Yaw  rate  (about  the  x  axis),  the  Pitch  rate 
(about  the  y  axis)  and  the  Roll  rate  (about  z  axis).  These 
rates  are  called  as  Ratel,  Rate2  and  Rnte3  for  link  one, 
link  two  and  link  three.  After  the  transformation  of 
velocities  to  the  Euler  rates,  they  can  be  directly 
integrated  to  get  the  Euler  angles.  In  this  thesis,  these 


angles  are  called  the  Yaw,  pitch  and  Roll  angles  about  th..  x 
y  z  axis  respectively  [Refs.  2,  20]. 

This  convention  is  very  important  and  should  not  be 
mixed  with  another  set  of  Euler  angles  described  differently 
in  the  literature  [Refs.  3,  20].  In  addition  to  that,  the 
order  of  the  rotation  must  be  decided  beforehand.  This  is 
true  because  the  orientation  of  objects  is  different  when 
they  are  rotated  in  a  different  order,  i.e. ,  first  the 
rotation  about  x  axis,  then  a  rotation  about  the  y  axis, 
finally  a  rotation  about  the  z  axis  will  produce  a  different 
orientation  in  space  than  the  one  which  was  defined  and  used 
in  this  thesis  (z,  y,  then  x).  The  transformation  matrices 
used  here  are  valid  as  long  as  the  assumed  order  of  the 
rotation  is  retained. 

In  the  literature,  a  quite  different  set  of  angles  is 
used  to  describe  the  orientation  [Refs.  2,  3].  While  some 
of  these  angles  define  the  orientation  with  respect  to  a 
non-orthogonal  coordinate  system  some  others  may  define  with 


respect  to 

an 

orthogonal  system. 

Euler 

angles 

define  an 

independent 

set 

of  coordinates 

system 

which 

are 

not 

orthogonal . 

Therefore,  all 

three 

coordinates 

are 

independent 

from 

i  each  other  and 

velocities 

in 

this 

coordinate  system  can  be  directly  integrated  to  get  the 
relevant  angles.  They  describe  the  unique  orientation  of 
the  body  in  space.  The  orthogonal  set  of  coordinate  axes  do 
not  form  an  independent  coordinate  system.  This  is  true 
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since  the  three  axis  have  a  certain  relation  with  each  other 
in  any  position,  i.e.,  direction  cosine  angles  have  a  unique 
relation  in  a  fixed  reference  system  and  cannot  be  obtained 
by  integrating  any  velocity  in  an  orthogonal  coordinate 
system.  The  velocities  in  an  orthogonal  coordinate  system 
must  thus  be  converted  to  a  nonorthogonal  coordinate  system 
(e.g.  Euler  angle  rates)  prior  to  integration. 

After  Yaw,  Pitch  and  Roll  angles  are  calculated,  it  is 
possible  to  go  back  and  express  the  orientation  of  the  body 
with  the  direction  cosines  in  an  orthogonal  coordinate 
system.  The  columns  of  the  transformation  matrix  from  one 
orthogonal  set  of  axes  to  another  describes  the  orientation 
of  the  new  coordinate  axis  with  respect  to  old  coordinate 
system.  So,  a  transformation  matrix  can  be  used  to  get  the 
direction  cosine  angles.  The  direction  cosines  of  each  link 
are  then  used  to  calculate  the  moment  of  inertia  of  the 
links.  The  variation  of  a  link  inertia  with  respect  to  time 
was  shown  in  Figure  4  as  it  was  calculated  during  a 
simulation  run.  The  derivation  of  the  transformation 
matrices  is  shown  in  Appendix  A. 

B.  CONSTRAINTS  IN  THE  SIMULATION  PROGRAM 

In  the  development  of  the  equations,  thus  far,  each 
link  has  been  treated  such  that  it  can  move  in  space  without 
any  constraint.  For  most  cases,  however,  degrees  of  freedom 
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of  each  link  must  be  reduced  so  that  the  link  can  move  only 
in  the  direction  permitted  by  its  joint. 

In  the  simulation  of  the  direct  dynamics  problem,  the 
base  rotation  is  transmitted  to  the  second  and  third  links 
for  the  three  revolute  joint  arm  which  was  studied.  This 
was  simulated  by  allowing  the  first  link  to  rotate  only 
about  Z  axis.  At  the  same  time,  the  rotational  rates  of  the 
second  and  third  links  about  the  Z  axis  were  made  equal. 

To  make  any  of  the  simulation  variables  zero,  meaning 
no  variation  in  that  direction,  one  zeroes  the  related  rows 
and  columns  in  MatA  putting  1  on  the  diagonal.  At  the  same 
time,  if  the  same  row  in  MatB  is  made  zero,  the 
corresponding  mathematical  expression  for  this  equation  will 
be  in  the  form  of  1  *  X  =  0 ,  and  a  result  of  this,  X  will  be 
equal  zero.  This  idea  can  also  be  applied  to  MatA  and  MatB 
to  make  two  variables  equal  so  that  XI  -  X2  =  0 .  Thus,  the 
above  motion  was  simulated  by  constraint. 

C.  PROGRAM  VALIDATION 

The  validation  of  the  inverse  dynamics  problem  has  been 
conducted  in  several  cases.  In  this  approach  the  idea  was 
to  choose  an  angular  acceleration  such  that  at  a  certain 
time,  two  of  the  three  links  would  align.  In  other  words, 
the  links  would  be  in  a  singular  position  at  this  time,  and 
if  the  simulation  procedure  worked,  the  singularity  problem 
would  have  been  avoided. 


The  validation  procedure  is  shown  in  Figure  5.  For 
this  procedure  link  two  angle  was  chosen  as  9  =  (Pi/2)  * 
sin(pi/2)  *  Time.  This  time  dependent  function  has  a  period 
of  4  sec  and  an  amplitude  of  90  degrees.  The  second 
derivative  of  this  function  is  9  =  -  ((pi**3)/8)  *  sin(pi/2) 
*  Time  and  corresponds  to  the  angular  acceleration  of  the 
link.  This  value  was  input  as  the  theoretical  angular 
acceleration  in  the  simulation  program,  and  corresponding 
linear  accelerations  and  forces  at  each  joint  were 
calculated.  The  other  two  links  were  forced  to.  have  zero 
rotational  velocity  throughout  the  simulation. 

To  apply  a  corresponding  torque  at  the  joint,  MaTA  and 
MatB  were  multiplied  and  a  right  hand  side  matrix  DQ  (27*1) 
was  obtained  (MATA  *  MATB  =  DQ) .  This  matrix  DQ  (27*1)  was 
used  to  solve  the  simulation  equations  in  the  form  of 
AX  =  DQ.  The  vector  X  (that  is,  theta)  was  fedback  in  the 
loop  and  the  theoretical  and  the  calculated  values  of  theta 
were  compared. 

The  above  discussion  has  been  implemented  in  three 
different  initials  configuration  as  shown  in  Figure  6.  To 
force  the  arm  links  to  the  various  singular  points,  several 
different  plane  motions  were  simulated.  For  each 
configuration,  three  different  angular  motions  were  input 
for  link  2,  or  as  can  be  seen  from  Figure  6,  for  each 
configuration,  one  angular  velocity  caused  a  spinning  motion 
of  the  link  about  the  axis  with  which  it  was  initially 
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L. 


aligned,  while  the  other  two  produced  a  plane  motion 
according  to  direction  of  the  applied  angular  motion.  The 
angles  between  two  successive  links  were  measured  for  each 
motion.  Figure  7  shows  the  angle  variation  between  link  1 
and  link  2,  and  link  2  and  link  3  corresponding  to  an 
angular  acceleration  applied  in  the  X  direction  for 
configuration  A.  As  can  be  seen  from  Figures  7  and  8,  two 
successive  links  pass  through  the  singular  points  every  2 
seconds,  i.e.,  they  align  and  the  angle  between  links 
becomes  either  0  or  180  degrees.  (The  singular  points  are 
marked  on  the  graph) .  Figure  8  shows  the  angle  variations 
for  an  angular  motion  applied  in  the  Z  direction  for 
configuration  A.  In  this  case,  it  is  obvious  that  the  angle 
between  link  1  and  link  2  is  always  constant  (90  degrees). 
The  second  graph  on  Figure  8  shows  the  angle  between  link  2 
and  link  3  now,  singularity  occurs  on  the  Z  motion,  with  the 
singularities  marked  as  in  Figure  8.  Figure  7  and  Figure  8 
are  representative  of  the  data  obtained  in  the  validation 
procedure  which  analyzed  nine  possible  motions  of  link  2 
leading  the  singularity.  This  data  showed  that  singularity 
in  these  directions  could  be  overcome,  and  a  solution  to  the 
problem  exists  using  this  approach. 

For  each  run,  the  error  between  the  theoretical  and  the 
simulated  value  of  Theta  was  computed.  Figure  9  shows  the 
percent  error  for  the  X  motion  for  configuration  A  (Figure  7 
Data).  The  trend  of  the  error  is  representative  of  every 
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Figure  9.  Percent  Error  Between  Theoretical  and 
Simulated  Angles 
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case  investigated.  The  figure  shows  that,  due  to  nature  of 
the  numerical  integration,  the  error  slightly  accumlates 
during  the  simulation,  but  still  has  very  small  value.  This 
proves  that  the  direct  dynamics  problem  can  be  solved  very 
accurately  by  Newton-Euler  approach  in  a  fixed  coordinate 
system. 
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IV. 


1.  A  dynamic  model  of  a  three  link,  rigid  revolute  joint 
manipulator  has  been  developed  in  this  thesis,  as  a 
general  computer  program  package. 

2.  Several  runs  for  different  initial  configurations  were 
simulated  and  the  singularity  problem  was  investigated. 
Theoretical  and  calculated  values  of  angular  positions 
were  compared.  It  was  proved  that  the  singularity 
problem  could  be  overcome  by  using  a  Newton-Euler 
approach  in  a  fixed  coordinate  system. 

3.  The  following  recommendations  are  provided: 

a.  Enhance  the  code  and  make  it  more  interactive. 
That  is,  let  the  user  specify  the  constraints  he 
wants  to  apply  on  each  link  by  answering 
interactive  questions  before  the  actual  simulation 
run  starts.  Thus,  the  motion  can  be  simulated 
with  different  constraints  without  going  into  the 
code  and  changing  the  relevant  parameters. 

b.  Adapt  the  code  for  use  in  a  microcomputer.  Add  a 
subroutine  in  the  program  to  invert  the  matrix  A. 
Thus,  the  code  will  be  more  independent  from 
outside  routines  and  more  adaptable  to  other 
computer  systems. 
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c.  Validation  of  the  approach  via  actual  experimental 
tests  in  crucial.  This  will  establish  a  way  of 
developing  accurate  constants  for  subsequent 
controller  design  and  provide  a  basis  for 
compensation  of  gravity  effects.  Determining 
these  constants  for  the  code  will  make  the 
simulation  program  more  concrete  and  will  provide 
more  physical  insight. 

d.  Finally,  develop  a  controller  for  a  manipulator 
which  makes  use  of  the  present  algorithm  for 
validation  and  design. 


DERIVATION  OF  THE  TRANSFORMATION  MATRIX  FROM 
EARTH  FIXED  COORDINATE  SYSTEM  TO  BODY  FIXED  COORDINATE  SYSTEM 

The  angular  velocity  terms  obtained  by  integration  of 
the  angular  acceleration  terms  are  with  respect  to  an  Earth 
fixed  coordinate  system.  To  define  the  Euler  angles  which 
are  called  Yaw  Pitch  and  Roll  angles  in  this  thesis,  we 
have  to  establish  an  appropriate  body  fixed  coordinate 
system.  Thus,  U,  V  and  W  is  a  right  hand  coordinate  system 
[Ref.  20]  with  its  origin  fixed  at  the  center  of  gravity  of 
a  link.  The  U,  V,  W  coordinate  system  is  initially  oriented 
such  that  the  angles  between  two  coordinate  system  axes  are 
simultaneously  reduced  to  zero,  i.e.,  i,  j  ,  k,  axis  are 
parellel  to  the  I,  J,  K  respectively. 

If  a  rotation  from  X  Y  Z  coordinate  system  to  the  U  V  W 
coordinate  system  is  accomplished  by  first  rotation  about  K 
axis  (roll),  then  about  J  axis  (pitch)  and  finally  about  I 
axis  (yaw),  it  follows  that  for  any  arbitrary  point  in  the  X 
Y  Z  coordinate  system,  the  corresponding  coordinates  in  the 
U  V  W  system  are; 


where  MatR  is  a  3*3  matrix. 


To  get  the  transformation  matrix,  we  need  to  examine  each 
rotation  separately. 

Rotation  about  the  Z  axis; 


a*Xc$  ♦  Ts  $ 
V.-X30*  Yc<)> 
w>  2 


u 

1 

o 

K* 

u 

« 

X 

V 

— 

-S^  0 

Y 

oof 

2 

_  _ 

— , 

Rotation  about  the  Y  axis; 

x  tiaXc0~z,se 


v.  Y 

Xse+2ce 


Rotation  about  the  X  axis; 


u.  X 

V.  Ycip  +  zsY 

W. -Ysfl+Zcf 


u 

1“ 

V 

— 

w 

_ 

By  multiplying  three  rotation  matrix  together; 


cecif 

ces^  "se 

MAT ft  • 

sysec.^- 

syses^+cyc^  5tjjc© 

cysecy  + 

Cyses^-syc^  eye© 

where 

c  =  cos 

S  =  Sin 

T  =  Tan 

The  transformation  matrix  from  body  fixed  to  Euler 
coordinate  system  is  obtained  as  below  [Ref.  20], 

’  0  C*  -St 

I  T6i  f  T6Cjj 

o  Se.ceSy'  Sec&cy 

The  angles  discussed  above  are  shown  in  Figure  10. 
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Figure  10.  Critical  Angles 

(a)  Euler  Angles,  Body  Coordinates 

(b)  Direction  Cosines,  Global  Coordinates 


APPENDIX  B 


THREE  DIMENSIONAL  DIRECT  DYNAMICS  SIMULATION  PROGRAM 


:!) 


TERMINAL 
METHOD  ADAMS 

PRINT  .05,DRCANX(l-3) ,DRCANY(l-3) ,DRCANZ(l-3) , . . . 

JXO , JYO , JZO , JX1 , JY1 , JZ1 , JX2 , JY2 , JZ2 , . . . 

LC0GX(l-3) ,LCOGY(l-3) ,LCOGZ(l-3) 

CONTROL  FINTIM  =2.0,  DELMAX  =.l,  DELPRT  =  .05 
D  DIMENSION  MATA(27 . 27 ) , MASS (3 , 2 ) , L (3 , 2 ) , RX(3 , 2) , RY(3 , 2) , RZ (3 , 2) 

D  DIMENSION  IXX(3 , 2) , IXZ(3 , 2) , IXY(3 , 2 ) , IYY( 3 , 2) , IYZ(3 , 2) , IZZ(3 , 2) 
D  DIMENSION  MAT1RC3 ,3 ) ,MAT2R( 3 , 3) ,MAT3R(3 ,3' 

D  DIMENSION  MAT1T(3,3) ,MAT2T(3, 3) ,MAT3  * 

D  INTEGER  IER, I,J,M,K,P,N,IA, IDGT, A 

EXCLUDE  IA , IDGT , IER , I , J , M , K , P , N . A 
ARRAY  MATB ( 27 )  . LCOGX ( 3 ) , LCOGY ( 3 ) , LCOGZ ( 3 ) 

ARRAY  VECTA0(3) , VECTBO (3 ) , VECTA1 (3 ) ,VECTB1(3) ,VECTA2(3) ,VECTB2(3) 
ARRAY  WDX(3 ) , WDY(3) , WDZ(3) ,W1(3),W2(3) , W3 ( 3) 

ARRAY  RATE1(3) ,RATE2(3) ,RATE3(3) ,BRATE1(3) ,BRATE2(3) ,BRATE3(3) 

ARRAY  RBG1(3) ,RAG1(3) ,RBG2(3) , RAG2 ( 3 ) , RBG3 (3 ) 

ARRAY  SUMHDX(3) ,SUMHDY(3) ,SUMHDZ(3) ,HDX(2) ,HDY(2) ,HDZ(2) ,WKAREA(850) 
ARRAY  IXXT  ( 3 )  ,  IYYT  ( 3 )  ,  IZZT  ( 3 ) ,  IXYT  ( 3 ) ,  IXZT  ( 3 ) ,  IYZT  ( 3 ) 

ARRAY  YAWANX ( 3  )  , PTCANY ( 3 ) , ROLANZ ( 3 ) 

ARRAY  DRCANX ( 3 ) , DRCANY { 3 ) , DRC ANZ ( 3 ) 

ARRAY  DRCRAX ( 3 ) , DRCRAY ( 3 ) , DRCRAZ ( 3 ) 

ARRAY  DRCSX(3) ,DRCSY(3) ,DRCSZ(3) 

D  DATA  MATA/ 7 29  *  0.0D0/ 


INITIAL 

*  INPUT  PARAMETER  CONSTANTS 

A  =  5.0D0 

P  =  0.0D0 

W  =  2.0D0  *  PI 

IDGT  =  3 

G=0 . 0D0 

N=27 

M=1 

IA  =27 

*  INPUT  JOINT  LOCATIONS  IN  METERS 

JXO  =  0.0D0 
JYO  =  O.ODO 
JZO  =  O.ODO 
JX1  =  O.ODO 
JY1  =  O.ODO 
JZ1  =  1.0D0 
JX2  =  O.ODO 
JY2  =  1.0D0 
JZ2  =  1.0D0 

*  INPUT  TORQUE  CONSTANTS 

TOX  =  O.ODO 
TOY  =  O.ODO 
TOZ  =  O.ODO 
T1Y  =  O.ODO 
T1Z  =  O.ODO 
T2Y  =  O.ODO 
T2Z  =  O.ODO 

*  INPUT  DISTANCE  FROM  CENTER  OF  LINK  TO  CENTER  OF  MASS 
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*  FOR  EACH  LINK  ENDS 

L(1 , 1)  =  0.50D0 

L(1 ,2 j  =  0.50D0 

L(2,l)  =  O.50DO 

L(2,2)  =  0.50D0 

L(3,l)  =  0.50D0 

L(3,2)  =  0.50D0 

*  INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 

MASS  (I ,  I )  =  2.5D0 
MASS (1,2)  =  2.5D0 
MASS (2 , 1 )  =  2.5D0 
MASS (2 , 2)  =  2.5D0 
MASSi 3 , 1 )  =  2.5D0 
MASS (3 ,2)  =  2.5D0 

*  INPUT  OMEGA  AND  OMEGA  DOT,  YAW,  PITCH,  AND  ROLL  ANGLES 

DO  30  I  =  1,3 

W1(I)  =  O.ODO 

W2(I)  =  O.ODO 

W3  ( I )  =  O.ODO 

WDX(I)  =  O.ODO 
WDY ( I )  =  O.ODO 

WDZ(I)  =  O.ODO 

YAWANX(I)  =  O.ODO 
PTCANY(I)  =  O.ODO 
ROLANZ(I)  =  O.ODO 

30  CONTINUE 

YWRX1  =  YAWANX ( 1 )  *  DEGRA 
PTRY1  =  PTCANY  1)  *  DEGRA 
RLRZ1  =  ROLANZ(l)  *  DEGRA 
YWRX2  =  YAWANX (2  *  DEGRA 

PTRY2  =  PTCANY  2  *  DEGRA 

RLRZ2  =  ROLANZ  2)  *  DEGRA 
YWRX3  =  YAWANX  3)  *  DEGRA 
PTRY3  =  PTCANY(3)  *  DEGRA 
RLRZ3  *  ROLANZ ( 3 )  *  DEGRA 

*  INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 

LCOGX(l)  =  O.ODO 
XI  =  LCOGX(l) 

LCOGY(l)  =  O.ODO 
Y1  =  LCOGY(l) 

LCOGZ(l)  =  0.5D0 
Z1  =  LCOGZ(l) 

LC0GX(2 )  =  O.ODO 
X2  =  LC0GX(2) 

LC0GY(2)  =  0.5D0 
Y2  =  LC0GY(2 ) 

LC0GZ(2)  =  l.ODO 
Z2  =  LCOGZ ( 2 ) 

LC0GX(3)  =  O.ODO 
X3  =  LCOGX( 3 ) 

LC0GY(3)  =  1.5D0 
Y3  =  LCOGY ( 3 ) 

LC0GZ(3)  =  l.ODO 
Z3  =  LC0GZ(3) 

*  INPUT  MASS  OF  EACH  LINK  IN  KG  AND  COMPUTE  WEIGHTS  IN  NEWTONS 

MASS1  =  5.0D0 
MASS2  =  5.0D0 
MAS S3  =  5.0D0 

WG1  =  MASSING 
WG2  =  MASS2*G 
WG3  =  MASS3*G 

*  INPUT  ACCELERATION  OF  JOINT  ZERO 
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* 


50 

40 


60 


* 


64 

63 


AOX  =  O.ODO 
AOY  =  O.ODO 
AOZ  =  O.ODO 


INITIALIZE  MATRIX  A  AND  B  TO  ZERO 

DO  40  I  *  1,27 
DO  50  J  =  1,27 

MATA (I , J)  =  O.ODO 
CONTINUE 
CONTINUE 


DO  60  I  =  1,27 
MATB(I)  =  O.ODO 
CONTINUE 


INITIALIZE  THE  TRANSFORMATION  MATRICIES  AND  VELOCITIES 
DO  63  I  =  1,3 
DO  64  J  =  1,3 


RATEl(I) 

= 

O.ODO 

RATE2 ( I ) 

= 

O.ODO 

RATE3( I) 

a 

O.ODO 

BRATEl(I) 

= 

O.ODO 

BRATE2  I) 

= 

O.ODO 

BRATE3(I) 

= 

O.ODO 

MAT IT  (I,J' 

)  = 

O.ODO 

MAT2T  (I,J 

a 

O.ODO 

MAT3T  (I,J 

a 

O.ODO 

MAT1R  (I,J 

a 

O.ODO 

MAT2R  (I,J 

a 

O.ODO 

MAT3R  (I,J 

a 

O.ODO 

CONTINUE 

CONTINUE 


DERIVATIVE 

NOSORT 

CALL  ERRSET  (208,256,-1,1,1) 
LEVELQ  =0  '  '  ' 

CALL  UERSET( LEVELQ, LEVLDQ) 

*  INITIALIZE  MATRIX  A  AND  B  TO  ZERO 


80 

70 


DO  70  I  =  1,27 
DO  80  J  =  1,27 

MATA ( I , J )  =  O.ODO 
CONTINUE 
CONTINUE 


90 


DO  90  I  =  1,27 
MATB(I)  =  O.ODO 
CONTINUE 


: k 
\ k 
* 


INPUT  JOINT  EQUATIONS 


JOINT  ZERO  EQUATIONS 


AB 


=  AG1  +  (WD1  X  RB/G1 )  +  W1  X  (W1  X  RB/G1 ) 

VECTAO ( 1 )  =  WDX{1) 

VECTAO  2  =  WDY ( 1 ) 

VECTAO (3)  =  WDZ(l) 


RBG1(1) 
RBG1 (2) 
RBG1(3) 


=  JXO  -  LCOGX(l) 
=  JYO  -  LCOGY(l) 
=  JZO  -  LCOGZ(l) 


CALL  CPROD (VECTAO , RBG1 , MIAO , MJAO , MKAO ) 

VECTAO ( 1 )  =  Wl(l) 

VECTAO ( 2 )  =  W1  2 
VECTAO (3)  =  W 1 ( 3 ) 


CALL  CPROD (VECTAO , RBG1 , MIBO , MJBO , MKBO ) 
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* 

it 


* 


it 


it 


* 


VECTBO ( 1 )  *  MIBO 
VECTBO (2)  *  MJBO 
VECTBO (3)  =  MKBO 

CALL  CPROD ( VECTAO , VECTBO ,MICO ,MJCO ,HKCO) 


JOINT  ONE  EQUATIONS  — 

AA  =  AG1  +  (WD1  X  RA/G1)  +  W1  X  (HI  X  RA/G1) 

VECTA1 (1 }  =  WDX(l) 

VECTA1 (2 }  =  WDY(l) 

VECTA1 (3)  =  WDZ(l) 


RAG1(1) 
RAG1(2 j 
RAG1(3) 


JX1  -  LCOGX(l) 
JY1  -  LCOGY(l) 
JZ1  -  LCOGZ(l) 


CALL  CPROD ( VECTA1 , RAG1 , MI A1 , MJA1 , MKA1 ) 


VECTAl (1)  =  Wl(l> 

VECTA1 (2)  =  Wl(2) 

VECTAl (3)  =  Wl(3) 

CALL  CPROD  (VECTAl ,RAG1 ,MIB1 ,MJB1 ,MKB1) 


VECTBl(l)  =  MIB1 
VECTB1 (2)  =  MJB1 
VECTB1 (3)  =  MKB1 

CALL  CPROD  (VECTAl, VECTB1 ,MIC1 ,MJC1 ,MKC1 ) 

AB  =  AG2  +  (WD2  X  RB/G2)  +  W2  X  (W2  X  RB/G2) 

VECTAl (1)  *  WDX(2) 

VECTAl (2  *  WDY ( 2 ) 

VECTAl (3)  =  WDZ(2) 


RBG2 

RBG2 

RBG2 


1)  =  JX1  -  LCOGX(2) 

2)  =  JY1  -  LCOGY ( 2 ) 

3)  =  JZ1  -  LCOGZ(2) 


CALL  CPROD  (VECTAl, RBG2 ,MIA2 ,MJA2 ,MKA2) 


VECTAl (1) 
VECTAl (2) 
VECTAl (3) 


*  W2 
=  W2 
=  W2 


1) 

2) 

3) 


CALL  CPROD  (VECTAl, RBG2 ,MIB2 ,MJB2 ,MKB2) 


VECTB1 (1 )  =  MIB2 
VECTB1 (2)  =  MJB2 
VECTB1 (3 )  *  MKB2 

CALL  CPROD  (VECTAl, VECTB1 ,MIC2 ,MJC2 ,MKC2) 


JOINT  TWO  EQUATIONS 

AA  =  AG2  +  (WD2  X  RA/G2)  +  W2  X  (W2  X  RA/G2) 

VECTA2(1)  =  WDX(2) 

VECTA2(2)  =  WDY ( 2 ) 

VECTA2 (3 )  =  WDZ(2) 

RAG2(1)  =  JX2  -  LCOGX(2 ) 

RAG2 ( 2 )  =  JY2  -  LCOGY  2) 

RAG2(3)  =  JZ2  -  LCQGZ (2) 

CALL  CPROD  (VECTA2 , RAG2 ,MIA3 ,MJA3 ,NKA3) 

VECTA2 ( 1 )  =  W2(l) 

VECTA2(2)  =  W2(2 
VECTA2(3)  =  W2(3) 

CALL  CPROD  (VECTA2 ,RAG2 ,MIB3 ,MJB3 ,MKB3) 

VECTB2(1 )  =  MIB3 
VECTB2(2)  =  MJB3 
VECTB2(3)  =  MKB3 

CALL  CPROD ( VECTA2 , VECTB2 , MIC3 , MJC3 , MKC3 ) 
AB  =  AG3  +  (WD3  X  RB/G3)  +  W3  X  (W3  X  RB/G3) 
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VECTA2{1 )  *  WDX(3) 

VECTA2(2)  =  WDY  3 
VECTA2(3)  =  WDZ(3) 

RBG3 ( 1 )  =  JX2  -  L'C0GX(3) 

RBG3(2)  =  JY2  -  LCOGY  3 
RBG3(3)  =  JZ2  -  LC0GZ(3) 

CALL  CPROD  ( VECTA2 , RBG3 , MIA4 , MKA4 , MKA4 ) 

VECTA2(1)  =  W3 (1 ) 

VECTA2 ( 2  J  =  W3  ( 2 ) 

VECTA2(3)  =  W3(3) 

CALL  CPROD  (VECTA2 , RBG3 , MIB4,MJB4 ,MKB4) 

VECTB2 ( 1 )  =  MIB4 
VECTB2 ( 2 )  =  MJB4 
VECTB2 (3 )  =  MKB4 

CALL  CPROD  (VECTA2 , VECTB2 ,MIC4,MJC4,MKC4) 

*  SUM  OF  MOMENTS  EQUATIONS 

DO  100  I  =  1,3 

*  COMPUTE  HX,H  DOT  X,HY,H  DOT  Y,  HZ,H  DOT  Z 


RX 

;i,i) 

=  “L(I , 1 ' 

*  DCOS (DRCRAX 

P 

) 

RX 

1,2 

=  L  1,2, 

*  DCOS (DRCRAX 

I 

) 

RY 

1,1 

=  -L( 1 , 1 

*  DCOS(DRCRAY 

T 

i 

RY 

12 

=  L(I ,2 

*  DCOS(DRCRAY 

T 

) 

RZ 

1,1 

=  -L  1,1 

*  DCOS (DRCRAZ 

T 

RZ 

1,2 

=  L(I , 2, 

*  DCOS (DRCRAZ 

T 

) 

)  =  MASS (I, 1) 

*  ( (RY 

1,1) 

*  RY(I,l)< 

+  ( RZ ( I , 1 ) 

*  gZfi'UN 

)  =  MASS (1,2) 

*  ((RY 

1,2) 

*  RY  ( 1 , 2 ) 

+  (RZ(I,2) 

*  RZ ( 1 , 2 ) ) ; 

IXX(l 

IXXT(I)  =  IXX(I,1)  +  IXXU, 
IF  (IXXT(I)  .LE.  .020)  THEN 
IXXT(I)  =  .020 
ELSE 

IXXT(I)  =  IXXT(I) 

END  IF 


IXY(I.l) 
1XY (1,2) 
IXYT(I) 

IXZ(I,1 
IXZ( I , 2 
IXZT(I) 

HDX(l) 
HDX ( 2 ) 

IYY (1,1) 
IVY (I. 2) 
IYYT(I) 


*  RX(I,1) 

_  *  RX(I,2 

=  IXY( I , 1 )  +  IXY( I , 2 ; 

)  *  RZ(I , 1 ] 

- *  RZ  1,2 

IXZ(I,1)  +  IXZ(I,2) 


MASS (I , 1 ) 
MASS (I, 2) 


MASS (1,1 
MASS (I, 2 


RY ( I , 1 ) 
RY ( I , 2 ) 


RX 

RX 


=  WDX(1 

)  *  IXX(I,1)-  WDZ(I 

)  *  IXZ 

1,1) 

-  WDY ( I 

)  *  IXY 

=  WDX(2 

)  *  I XX (I, 2)-  WDZ( I 

)  *  IXZ 

1,2) 

-  WDY ( I 

)  *  IXY( 

=  MASS (1,1) 
=  MASS (I, 2) 


((RX(I,1 
( (RX( I , 2 


. .  =  IYY (1,1)  +  IYY (1,2) 

IF  (IYYT(I)  .LE.  .020)  THEN 

IYYT(I)  =  .020 

ELSE 

IYYT(I)  =  IYYT(I) 

END  IF 

S(I,1)  =  MASSI 
'(1,2  =  MASSI 

-  =  iyz(: 


RX( 1 , 1 ) )  +  ( RZ  ( 1 , 1 ) 
RX ( I , 2 ) )  +  ( RZ  ( 1 , 2 ) 


*  RZ  ( 

*  RZ 


IYZI 

IYZ( _ 

IYZT(I) 


*  RY(I.l) 

*  RY  1,2 

+  iyz(i,2; 


RZ 

RZ 


(i:ii 


(iJ) 

l-M 


)  =  WDY ( I 

)  *  IYY(I,1) 

-  WDX(I 

)  *  IXY 

1,1) 

-  WDZ(I 

)  =  WDY(I 

)  *  IYY (I ,2) 

-  WDX(I 

)  *  IXY 

1,2) 

-  WDZ(I 

)  *  IYZ ( 

fttl 


izz  1,1 
IZZ(I,2) 
IZZT(I) 
IF  (IZZT 


=  MASS (I , 1 
=  MASS (I, 2 
=  IZZ(I,1) 
(I)  .LE.  ‘ 


RX ( 1 , 1 ) 
RX  1,2) 
1,2^ 


*  RX(I,1))  + 

*  RX ( I , 2 ) )  + 


RY ( 1 , 1 ) 
RY(I , 2) 
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IZZT(I)  =  .020 
ELSE 

IZZT(I)  =  IZZT(I) 
END  IF 


HDZ(1 

)  =  WDZ(I 

)  '*  IZZ 

'1,1) 

-  WDXfl 

)  *  IXZ< 

-  WDY ( I 

)  *  IYZ( 

HDZ(2 

)  =  WDZ(I 

)  *  IZZ 

,1,2} 

-  WDX(I 

)  *  IXZ( 

1.2) 

-  WDY (I 

)  *  IYZ( 

SUMHDX(I)  =  HDX(l)  +  HDX(2) 

SUMHDY(I)  =  HDY  l)  +  HDY { 2 ) 

SUMHDZ(I)  =  HDZ(l)  +  HDZ(2) 

100  CONTINUE 

*  ENTER  CONSTANTS  INTO  MATRIX  A 

*  LINK  ONE 

*  SUM  OF  FORCES  IN  THE  X  DIRECTION 

MATA (1,1)  =  1.0D0 

MATA (1,4)  =  MASS1 

MATA ( 1 ,10)  =  -1.0D0 
MATB(l)  =  0.0D0 

*  SUM  OF  FORCES  IN  Y  DIRECTION 

MATA (2,2)  =  1.0D0 

MATA(2,5)  =  MASS1 

MATA(2 , 11 )  =  -1.0D0 

MATB (2)  =  0.0D0 

*  SUM  OF  FORCES  IN  Z  DIRECTION 

MATA (3, 3)  =  1.0D0 

MATA  ( 3 , 6 )  =  MASS1 

MATA ( 3 ,12)  =  -1.0D0 

*  SUM  OF  FORCES  LINK  ONE  EQUAL 

MATB (3)  =  -WG1 

*  EQUATIONS  AT  JOINT  ZERO 

*  IN  THE  X  DIRECTION 

MATA (4, 4)  ■  1.0D0 

MATA (4 , 8 )  *  RBG1(3) 

MATA(4 , 9 )  =  -RBG1(2) 

MATB (4)  =  AOX  -  MICO 

*  IN  THE  Y  DIRECTION 

MATA ( 5 , 5 )  =  1.0D0 

MATA { 5 , 7 )  =  -RBG1 (3) 

MATA (5 ,9)  =  RBG1(1) 

MATB ( 5 )  =  AOY  -  MJCO 

*  IN  THE  Z  DIRECTION 

MATA (6,6)  =  1.0D0 

MATA (6,7)  =  RBG1 (2 ) 

MATA (6,8)  =  -RBGl(l) 

MATB (6)  =  AOZ  -  MKCO 

*  SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  ONE  IN  THE  X,Y,Z  DIRECTIONS 

MATA(7 , 2)  =  RBG1 ( 3 ) 

MATA (7 , 3 )  =  -RBG1(2 

MATA( 7 , 7 )  =  -IXXT(1 

MATA (7 , 8)  =  I XYT ( 1 ) 

MATA (7 , 9 )  =  IXZT(l) 

MATA (7 ,11)  =  -RAG1(3 
MATA(7 ,12)  =  RAG1(2) 

MATB ( 7 )  =  T1X  -  TOX 

MATA{8,1)  =  -RBG1 (3 ) 
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k 

k 

k 

k 

k 

k 

k 

k 

k 


MATA(8,3)  =  RBG1(1) 

MATA (8,7)  =  IXYT(l) 

MATA(8,8)  =  -IYYT(l) 

MATA (8, 9)  =  lYZT(l) 

MATA(8 , 10)  =  RAG1 (3 ) 

MATA(8,12)  =  -RAG1(1) 

MATS ( 8 )  =  T1Y  -  TOY 

MATA(9 , 1 )  =  RBG1 (2) 

MATA(9,2)  =  -RBG1  1 

MATA(9,7)  =  IXZT(l)  +  IXZT(2)  +  IXZT(3) 

MATA (9 ,8)  =  IYZT(l)  +  IYZT(2)  +  IYZT  3) 

MATA (9,9)  =  -IZZT  1)  -  IZZT(2)  -  IZZT(3) 

MATA (9 , 10)  =  -RAG1(2) 

MATA(9 , 11 )  =  RAG1(1) 

MATB(9)  =  T1Z  -  TOZ 
LINK  TWO 

SUM  OF  FORCES  IN  X  DIRECTION 

MATA(10 , 10)  =  1.0D0 

MATA( 10 , 13 )  =  MASS2 
MATA( 10 ,  19 )  =  -1.0D0 

MATB (10)  =  O.ODO 

SUM  OF  FORCES  IN  THE  Y  DIRECTION 

MATA( 11 ,  11 )  =  1.0D0 

MATA ( 11 , 14  )  =  MASS2 
MATA( 11 , 20 )  =  -1.0D0 

MATB (11)  =  O.ODO 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 

MATA (12, 12)  =  l.ODO 
MATA ( 12 ( 15 )  =  MASS 2 
MATA(12,21)  =  -l.ODO 

SUM  OF  FORCES  LINK  TWO  EQUAL 
MATB (12)  =  -WG2 

EQUATIONS  AT  JOINT  ONE 
IN  THE  X  DIRECTION 

MATA(13 ,4)  =  -l.ODO 

MATA( 13 , 8 )  =  -RAG1 (3) 

MATAi 13,9)  =  RAG1 ( 2 ) 

MATA( 13,13)  =  l.ODO 
MATA( 13,17)  =  RBG2(3) 

MATA (13 ,18)  =  -RBG2(2) 

MATB (13)  =  MIC1  -  MIC2 

IN  THE  Y  DIRECTION 

MATA (14 , 5 )  =  -l.ODO 

MATA( 14 , 7 )  =  RAG1 ( 3 ) 

MATA( 14 , 9 )  =  -RAGl(l) 

MATA( 14 , 14)  =  l.ODO 
MATA (14, 16;  =  -RBG2(3) 

MATA(14 ,18)  =  RBG2(1) 

MATB (14)  =  MJC1  -  MJC2 

IN  THE  Z  DIRECTION 

MATA(15 ,6)  =  -l.ODO 

MATA(15,7  =  -RAG1 ( 2 ) 

MATA  15,8)  =  RAG1(1) 

MATA( 15 , 15)  =  l.ODO 
MATA ( 1 5,16)  =  RBG2 ( 2 ) 

MATA( 15 , 17 )  =  -RBG2 ( 1 ) 

MATB (15)  =  MKC1  -  MKC2 

SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  TWO  IN  THE  X..Y..Z  DIRECTIONS 
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* 
i k 
ik 
* 
A 
A 
A 

A 

A 

A 


A 


A 


A 


A 


A 


MATA ( 16 , 11 ) 
MATA  16,12 
MATAi 16 ,  16 ) 
MATA  16,17) 
MATA (16 ,18) 
MATA(16,20 
MATA(16,21) 

MATS (16) 

MATA( 17,10) 
MATAi 17 , 12) 
MATAi 17 , 16) 
MATAi 17 , 17 ) 
MATA (17 ,18) 
MATAi 17 , 19) 
MATA(17 , 21 ) 

MATB(17 ) 

MATA( 18,9) 
MATAi 18 , 18) 
MATB(18) 

MATA (18,10)  = 
MATA (18 , 11 )  = 
MATA (18, 16)  = 
MATA (18 , 17 )  = 
MATA(18,18)  = 
MATA1I8, 19) 
MATA(18,20) 

MATB(18)  = 

LINK  THREE 
SUM  OF  FORCES 

MATA (19 ,19) 
MATA(19 , 22 ) 

MATB(19) 

SUM  OF  FORCES  IN 

MATA ( 20 , 20 )  = 
MATA (20, 23)  * 

MATB(20)  5 
SUM  OF  FORCES  IN 

MATA (21 ,21) 
MATA(21 ,24) 

MATB(21) 


=  RBG2 ( 3 ) 

=  -RBG2(2) 

=  -IXXT(2) 

=  IXYT  2) 

=  IXZT(2) 

=  -RAG2  3) 

=  RAG2 ( 2 ) 

=  (-T1X  +  T2X) 

=  -RBG2(3) 

=  RBG2 ( 1 ) 

=  IXYT(2) 

=  -IYYT(2) 

=  IYZT (2 ) 

=  RAG2 (3) 

-  -RAG2(1) 

=  (-  T1Y  +  T2Y) 

=  -1.0D0 
=  1.0D0 

=  0.0D0 


RBG2 1 

(2; 

> 

-RBG2i 

1 

» 

IXZTl 

2 

V  j 

1  +  IXZT(3 

IYZTI 

2' 

1  +  IYZT (3 

-IZZTI 

2 

1  -  IZZT(3 

-RAG2 ( 

2 

1 

RAG2  ( 

1 

1 

T1Z  +  T2Z 


1.0D0 

MASS3 

0.0D0 

THE  Z  DIRECTION 

1.0D0 

MASS3 

WG3 


IN  THE  X  DIRECTION 

1.0D0 
MASS3 

0.0D0 

THE  Y  DIRECTION 


EQUATIONS  AT  JOINT  TWO 
IN  THE  X  DIRECTION 


MATA (22 ,13)  =  -1.0DQ 
MATAi 22 , 17 )  =  -RAG2(3) 
MATAi 22 , 18)  =  RAG2(2 
MATA (22,22)  =  1.0 DO 

MATA (22 , 26 )  =  RBG3 ( 3 ) 
MATA(22 , 27 )  =  -RBG3(2) 

MATB(22)  =  MIC3  -  MIC4 
IN  THE  Y  DIRECTION 


MATA(23 , 14) 
MATA(23, 16) 
MATA (23, 18) 
MATA (23, 23) 
MATAi 23 , 25 ) 
MATA (23, 27) 

MATB(23) 


-1 .0D0 
RAG2(3) 
-RAG2  l) 
1.0D0 
-RBG3 ( 3 ) 

RBG3 ( 1 ) 

MJC3  -  MJC4 


DCOS(RLRZl) 


*  DSIN(RLRZl) 
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IN  THE  Z  DIRECTION 


MATA(24, 15)  = 

-1.0D0 

MATA(24,16)  = 

-RAG2(2) 

MATA(24,17  = 

RAG2(1) 

MATA (24, 24)  = 

1.0D0 

MATA (24 ,  25 )  = 

RBG3(2) 

MATA(24,26)  = 

-RBG3 ( 1 ) 

MATB(24) 

MKC3  -  MKC4 

SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  THREE  IN  THE  X,Y,Z  DIRECTIONS 

MATA(25 , 20)  =  RBG3(3) 

MATA (25 ,  21 )  =  -RBG3(2 
MATA(25 , 25 )  =  -IXXT(3) 

MATA (25 , 26 )  =  IXYT(3 
MATA (25 , 27 )  =  IXZT(3) 

MATB(25)  =  -T2X  *  DCOS(RLRZl) 

MATA(26 , 19)  =  -RBG3(3) 

MATA(26/21)  =  RBG3(1) 

MATA (26 , 25 )  =  IXYT(3 
MATA( 26 , 26 )  =  -IYYT(3) 

MATA(26,27)  =  IYZT(3) 

MATS (26)  =  -T2Y  *  DSIN(RLRZl) 


MATA(27 ,9)  =  -1.0D0 
MATA(27.27)  =  1.0D0 
MATB(27)  =  O.ODO 


★ 

MATA(27 , 19 ) 

= 

RBG3(2) 

i k 

MATA(27 , 20 

= 

-RBG3 ( 1 ) 

k 

MATA (27 ,25 

IXZT (3 ) 

k 

MATA(27 , 26 

= 

IYZT(3) 

k 

k 

MATA(27 , 27 ) 

= 

-IZZT(3) 

k 

MATB(27) 

= 

-  T2Z 

GO  TO  1112 


*  INITIALIZE  MATRIX  ACCORDING  TO  CONSTRAINTS 

*  CONSTRAINTS  GROUP  1  WHEN  ONLY  LINK  THREE  IS  IN  MOTION 

*  DO  118  I  =  1,18 

*  DO  18  J  =  1,27 

*  MATA ( I , J )  =  0.0 

*  MATA(I.I)  =1.0 

*  MATB(I)  =  0.0 

*18  CONTINUE 

*118  CONTINUE 

* 

*  DO  181  I  =  19,27 

*  DO  81  J  =  1,18 

*  MATA( I , J )  =  0.0 

*81  CONTINUE 

*181  CONTINUE 

*  GO  TO  1111 

*  CONSTRAINTS  GROUP  2  WHEN  LINK  TWO  AND  THREE  ARE  IN  MOTION 


k 

k 

DO  19  I  ! 
DO  191 

■  1.9 

J  =  1 

,27 

k 

MATAI 

;!,j) 

=  O.ODO 

k 

MATAl 

1, 1 

=  1.0  DO 

k 

k 

MATBI 

[ij 

=  O.ODO 

k 

mATAI 

;i7,j) 

=  O.ODO 

k 

MATAl 

18(J 

=  O.ODO 

k 

MATBI 

17 

=  O.ODO 

k 

£ 

MATBI 

18) 

=  0  .DO 

★ 

MATA( J , 17 ) 

=  O.ODO 

F 


i 


★ 

★ 

* 

* 

* 

*191 

*19 

k 

k 

k 

k 

*92 

*91 

** 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 


MATA(J,18)  =  O.ODO 

mATA( 17,17)  =  l.ODO 
MATA (18, 18)  =  l.ODO 

CONTINUE 

CONTINUE 


DO  91  I  =  10,27 
DO  92  J  =  1,9 
MATA (I , J)  =  0.0 
CONTINUE 
CONTINUE 
GO  TO  1111 

CONSTRAINTS  GROUP  3  WHEN  THREE  OF  THE  LINKS 

DO  78  J  =  1,27 

MATA {7 , J)  =  O.ODO 
MATA(8,J)  =  O.ODO 
MATA(J,7  =  O.ODO 
MATA(J,8)  =  O.ODO 
MATB(7  =  O.ODO 
MATB ( 8 )  =  O.ODO 

MATA ( 1 7 , J )  =  O.ODO 
MATA ( 18 , J )  =  O.ODO 
MATA( J , 17 )  =  O.ODO 
MATA( J , 18)  =  O.ODO 
MATB (17)  =  O.ODO 

MATB (18)  =  O.ODO 

MATA (26 , J)  =  O.ODO 
MATA(27 , J)  =  O.ODO 
MATA(J,26  =  O.ODO 
MATA( J,27)  =  O.ODO 
MATB (26)  =  O.ODO 

MATB (27 )  =  O.ODO 

MATA (7 , 7 )  =  l.ODO 

MATA ( 8 , 8 )  =  l.ODO 

MATA( 17 , 17 )  =  l.ODO 
MATA(18, 18)  =  l.ODO 
MATA(26,26)  =  l.ODO 
MATA(27,27)  =  l.ODO 


ARE  IN  MOTION 


*78  CONTINUE 


I 


*  CONSTRAINT  GROUP  4  THE  FIRST  LINK  IS  CONSTRAINED  TO  ROTATE  IN  Z  DIR. 

1112  DO  48  I  =  4,8 

DO  481  J  =  1,27 

MATA(I , J)  =  O.ODO 
MATA(I.I)  =  1.0  DO 
MATB(i)  =  O.ODO 
481  CONTINUE 

48  CONTINUE 

DO  84  I  =  9,27 
DO  841  J  =  4,8 
MATA ( I , J )  =  0.0 
841  CONTINUE 

84  CONTINUE 

*  CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

1111  CALL  LEQT2F (MATA,M,N, IA ,MATB , IDGT , WKAREA, IER) 

*  IF  (IER  .NE.  0)  CALL  ENDJOB 


FIND  LCOGX , LCOGY , LCOGZ , THETA  VALUES ,WX,WY,WZ 
LINK  ONE 

AX1  =  MATB ( 4 ) 
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*  *  *  *  * 


606 

605 


★ 


VELX1 

S 

INTGRL(0 . , AX1 ) 

LCOGX1 

= 

INTGRL (XI ,VELX1 

LCOGX(l) 

a 

LCOGX1 

AY1 

= 

MATB(5) 

VELY1 

= 

INTGRL(0. ,AY1) 

LCOGY1 

s 

INTGRL(Y1 ,VELY1 

LCOGY(l) 

= 

LCOGY1 

AZ1 

a 

MATB(6) 

VELZ1 

a 

INTGRLfO. , AZ1 ) 

LCOGZ1 

= 

INTGRL (Z1 , VELZ1 

LCOGZ(l) 

a 

LCOGZ1 

WD1X 

= 

MATB ( 7 ) 

W1X 

a 

INTGRL(0 . , WD1X) 

WDX(l) 

a 

WD1X 

Wl(l) 

= 

W1X 

WD1Y 

a 

MATB(8) 

W1Y 

= 

INTGRL ( 0. ,WD1Y) 

WDY(l) 

WD1Y 

Wl(2) 

= 

WIY 

WD1Z 

= 

MATB{9) 

W1Z 

= 

INTGRL(0 . , WD1Z) 

WDZ(l) 

2= 

WD1Z 

W1  (3) 

S 

W1Z 

TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COORDINATE 
SYSTEM  FOR  LINK  ONE 


MATlR(l.l)  =  DCOS(RLRZl)  *  DCOS(PTRYl) 

MAT1R( 2 , 1 )  =  DCOS(RLRZl)  *  DSIN(PTRYl)  *  DSIN(YWRXl) 
DSIN(RLRZl)  *  DCOS(YWRXl) 

MAT1R(3 , 1 )  =  DCOS(RLRZl)  *  DSIN(PTRYl)  *  DCOS(YWRXl)  +  ... 
DSIN(RLRZl)  *  DSIN(YWRXl) 

MAT1R(1 ,2)  =  DSIN(RLRZ1 ) *DCOS (PTRY1 ) 

MAT1R(2 , 2)  =  DSIN(RLRZl)  *  DSIN(PTRYl)  *  DSIN(YWRXl)  +... 
DCOS(RLRZl)  *  DCOS(YWRXl) 

MAT1R(3 , 2 )  =  DSIN(RLRZl)  *  DSIN(PTRYl)  *  DCOS(YHRXl) 
DCOS(RLRZl)  *  DSIN(YWRXl) 

MAT1R(1 ,3)  =  -DSIN(PTRYl) 

MATIR(2 ,3)  =  DCOS(PTRYl)  *  DSIN(YWRXl) 

MAT1R(3 , 3 )  =  DCOS(PTRYl)  *  DCOS(YWRXl) 

GET  THE  VELOCITIES  OF  LINK  ONE  IN  BODY  FIXED  COORDINATE  SYSTEM 


DO  605  J  *  1,3 
SUMI  =  0.0D0 
DO  606  K  =  1,3 

SUMI  =  SUM1  +  MAT1R( J,K)  *  W1(K) 
CONTINUE 

BRATEl(J)  =  SUMI 
CONTINUE 


TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  NON-OFTHOGONAL  EULER 
COORDINATE  SYSTEM  FOR  LINK  ONE 

MATlT(l.l)  *  0.0D0 
MAT IT (2,1)  =  1.0D0 
MAT 1 T ( 3 , 1 )  =  0.0D0 

MAT IT (1,2)  =  DCOS(YWRXI) 

MATH  2,2  =  DTAN(PTRYl)  *  DSIN(YWRXl) 

MAT IT (3,2)  =  1 . ODO/DCOS (PTRY1 )  *  DSIN(YWRXl) 

MAT1T{ 1 , 3 )  =  -DSIN(YWRXl) 

MAT1T  2,3)  =  DTAN(PTRYl)  *  DCOS(YWRXl) 

MAT1T( 3,3)  =  1 . DO/DCOS (PTRY1 )  *  DCOS(YWRXl) 

GET  THE  VELOCITIES  OF  LINK  ONE  IN  THE  EULER  COORDINATE  SYSTEM 
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*  * 


DO  705  J  =  1,3 
SUM1  *  0.0D0 
DO  706  K  -  1,3 

SUM1  =  SUM1  +  MAT1T(J,K)  *  BRATEl(K) 

706  CONTINUE 

RATEl(J)  *  SUM1 
70S  CONTINUE 

RATE IX  =  RATEI (1) 

RATE1Y  =  RATEI (2) 

RATE12  =  RATEI (3) 

*  INTEGRATION  OF  THE  VELOCITIES  OF  LINK  ONE  IN  EULER  COOR.  SYSTEM 

YWRX1  *  INTGRL(0. /RATE1X) 

PTRY1  *  INTGRL ( 0. ,RATE1Y) 

RLR21  *  INTGRL(-PI/2. ,RATE1Z) 

*  CONVERT  THE  ANGLES  TO  DEGREES 

YAWANX(l)  =  YWRX1  *  RADEG 

PTCANY(l)  =  PTRY1  *  RADEG 

ROLANZ ( 1 )  *  RLRZ1  *  RADEG 

*  GET  THE  DIRECTION  COSINES  FOR  THE  LINK  ONE 

DRCSY(l)  =  DCOS(RLRZl)  *  DSIN(PTRYl)  *  DCOS(YWRXl)  +... 
DSIN(RLRZl)  *  DSIN(YWRXl) 

DRCSX(l)  *  DSIN(RLRZl)  *  DSIN(PTRYl)  *  DCOS(YWRXI) 

DCOS (RLRZ1)  *  DSIN(YWRXl) 

DRCSZ(l)  *  DCOS (PTRY1 )  *  DCOS(YWRXl) 

DRCRAX(I)  =  DACOS (DRCSX( 1 ) ) 

DRCRAY(l)  =  DACOS (DRCSY(l)) 

DRCRAZ(l)  =  DACOS (DRCSZ( 1 ) ) 

DRCANX(l)  =  DACOS (DRCSX(l))  *  RADEG 
DRCANY(l)  =  DACOS ( DRCSY ( 1 ) )  *  RADEG 
DRCANZ(l)  *  DACOS (DRCSZ(l))  *  RADEG 

*  LINK  TWO 

9  AX2  *  MATB(13) 

VELX2  =  INTGRL (0 . ,AX2) 

LCOGX2  =  INTGRL ( X2 ( VELX2 ) 

LCOGX(2)  =  LCOGX2 

AY2  =  MATB( 14) 

VELY2  »  INTGRL ( 0. ,AY2) 

LCOGY2  *  INTGRL <Y2,VELY2) 

LCOGY ( 2 )  =  LCOGY2 

AZ2  *  MATB (15) 

VELZ2  *  INTGRL (6. ,AZ2) 

LCOGZ2  =  INTGRL (Z2,VELZ2) 

LCOGZ ( 2 )  *  LCOGZ2 

WD2X  =  MATS (16) 

W2X  a  INTGRL ( 6. ,WD2X) 

WDX(2)  *  WD2X 

W2(l )  «  W2X 

WD2Y  ■  MATB (17) 

W2Y  a  INTGRL { 6. ,WD2Y) 

WDY(2)  »  WD2Y 

W2(2)  *  W2Y 

WD2Z  *  MATB(18) 

W2Z  a  INTGRL ( 0. ,WD2Z) 

WDZ<2)  =  WD2Z 

W2(3)  =  W2Z 

TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COORDINATE 
SYSTEM  FOR  LINK  TWO 

MAT2R(1,1)  =  DCOS(RLRZ2)  *  DCOS(PTRY2) 
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*  * 


MAT2R(2,1)  =  DC0S(RLRZ2)  *  DSIN(PTRY2)  *  DSIN(YWRX2) 
DSIN(RLRZZ)  *  DCOS (YWRX2 ) 

MAT2R(3 , 1 )  =  DCOS (RLRZ2 )  *  DSIN(PTRY2)  *  DCOS(YWRX2)  +... 
DSIN(RLRZ2)  *  DSIN(YWRX2) 

MAT2R(1,2)  =  DSIN(RLRZ2)  *  DCOS(PTRY2) 

MAT2R(2 ,2)  =  DSIN(RLRZ2)  *  DSIN(PTRY2)  *  DSIN(YWRX2)  +... 
DCOS (RLRZ2)  *  DCOS(YWRX2) 

MAT2R( 3 , 2 )  =  DSIN(RLRZ2)  *  DSIN(PTRY2)  *  DCOS(YWRX2) 
DCOS(RLRZ2)  *  DSIN(YWRX2) 

MAT2R(1 ,3)  =  -DSIN(PTRY2) 

MAT2R(2,3)  =  DCOS(PTRY2)  *  DSIN(YWRX2) 

MAT2R(3,3)  =  DCOS(PTRY2)  *  DCOS(YWRX2) 

GET  THE  VELOCITIES  OF  LINK  TWO  IN  BODY  FIXED  COORDINATE  SYSTEM 

DO  607  J  =  1,3 
SUM1  =  0.0D0 
DO  608  K  =  1,3 

sum  =  sum  +  mat2R(j,k)  *  w2(k) 

608  CONTINUE 

BRATE2( J)  =  SUM1 
607  CONTINUE 

TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  NON-ORTHOGONAL  EULER 
COORDINATE  SYSTEM  FOR  LINK  TWO 

MAT2T (1,1)  =  0.0D0 
MAT2T (2,1)  =  1.0D0 
MAT2T(3 , 1 )  =  0.0D0 

MAT2T (1,2)  =  DCOS (YWRX2 ) 

MAT2T (2 , 2 )  =  DTAN(PTRY2)  *  DSIN(YWRX2) 

MAT2T (3,2)  =  1 . ODO/DCOS (PTRY2 )  *  DSIN(YWRX2) 

MAT2T (1.3)  *  -DSIN(YWRX2) 

MAT2T (2 , 3 )  *  DTAN(PTRY2)  *  DCOS(YWRX2) 

MAT2T(3,3)  *  i .ODQ/DCOS(PTRY2)  *  DCOS(YWRX2) 

GET  THE  VELOCITIES  OF  LINK  TWO  IN  THE  EULER  COORDINATE  SYSTEM 

DO  707  J  =  1,3 
SUM1  =  O.ODO 
DO  708  K  =  1,3 

SUM1  =  SUM1  +  MAT2T(J,K)  *  BRATE2(K) 

708  CONTINUE 

RATE2( J)  =  SUM1 
707  CONTINUE 

RATE2X  =  RATE2{ 1 ) 

RATE 2 Y  =  RATE2  2 
RATE2Z  =  RATE2(3) 

INTEGRATION  OF  THE  VELOCITIES  OF  LINK  TWO  IN  EULER  COOR.  SYSTEM 

YWRX2  =  INTGRL{0 . ,RATE2X) 

PTRY2  =  INTGRL(0. ,RATE2Y) 

RLRZ2  =  INTGRL( -PI/2 . ,RATE2Z) 

CONVERT  THE  ANGLES  TO  DEGREES 

YAWANX(2)  =  YWRX2  *  RADEG 
PTCANY ( 2 }  =  PTRY2  *  RADEG 
ROLANZ ( 2 )  =  RLRZ2  *  RADEG 

GET  THE  DIRECTION  COSINES  FOR  THE  LINK  TWO 

DRCSY ( 2 )  =  DCOS (RLRZ2 )  *  DSIN(PTRY2)  *  DSIN(YWRX2) 
DSIN(RLRZ2)  *  DCOS(YWRX2) 

DRCSX(2)  =  DSIN(RLRZ2)  *  DSIN(PTRY2)*DSIN(YWRX2)  +... 

DCOS (RLRZ2 )  *  DCOS(YWRX2) 

DRCSZ(2)  =  DCOS (PTRY2 )  *  DSIN(YWRX2) 

DRCRAX(2)  -  DACOS(DRCSX(2} ) 
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*  -1C 


DRCRAY ( 2 )  =  DACOS ( DRCS Y ( 2 ) } 

DRCRAZ(2)  =  DAC0S(DRCSZ(2)) 

DRCANX(2 )  =  DACOS(DRCSX(2) )  *  RADEG 
DRCANY ( 2 )  =  DACOS(DRCSY(2  *  RADEG 
DRCANZ(2)  =  DACOS (DRCSZ(2) )  *  RADEG 

JX1  =  (L(l , 1)  +  L(1 , 2) }  *  DCOS(DRCRAX(l)) 

JY1  =  Ll,l  +L  1,2  *  DCOS(DRCRAY(l)j 

JZ1  =  (L( 1 , 1 )  +  L(1 ,2) )  *  DCOS ( DRCRAZ ( 1 ) ) 

*  LINK  THREE 

6  AX3  *  MATB ( 22 ) 

VELX3  =  INTGRL(0 . ,  AX3 ) 

LCOGX3  =  INTGRL ( X3 , VELX3 ) 

LCOGX(3)  =  LCOGX3 

AY 3  =  MATB (23) 

VELY3  =  INTGRL ( 0. (AY3) 

LCOGY3  =  INTGRL (Y3,VELY3) 

LCOGY (3)  =  LC0GY3 

AZ3  =  MATB (24) 

VELZ3  =  INTGRL ( 0. ,AZ 3) 

LCOGZ3  =  INTGRL (Z3,VELZ3) 

LCOGZ(3 )  =  LC0GZ3 

WD3X  =  MATB (25) 

W3X  =  INTGRL (0. ,WD3X) 

WDX( 3 )  =  WD3X 

W3 ( 1 )  =  W3X 

WD3Y  =  MATB (26) 

W3Y  =  INTGRL ( 0. ,WD3Y) 

WDY ( 3 )  =  WD3Y 

W3(2)  =  W3Y 

WD3Z  =  MATB (27) 

W3Z  =  INTGRL ( 0. ,WD3Z) 

WDZ(3 )  =  WD3Z 

W3(3)  =  W3Z 

TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COORDINATE 
SYSTEM  FOR  LINK  THREE 

MAT3R( I , 1 )  =  DCOS (RLRZ3)  *  DCOS(PTRY3) 

MAT3R( 2 , 1 )  =  DCOS(RLRZ3)  *  DSIN(PTRY3)  *  DSIN(YWRX3) 
DSIN(RLRZ3 )  *  DCOS(YWRX3) 

MAT3R(3 , 1 )  =  DCOS ( RLRZ3 )  *  DSIN(PTRY3)  *  DCOS(YWRX3)  +... 
DSIN(RLRZ3)  *  DSIN(YWRX3) 

MAT3R( 1 , 2 )  =  DSIN(RLRZ3)  *  DCOS(PTRY3) 

MAT3R(2 , 2)  =  DSIN(RLRZ3)  *  DSIN(PTRY3)  *  DSIN(YWRX3)  +... 
DCOS (RLRZ3 )  *  DCOS(YWRX3) 

MAT3R(3 ,2)  =  DSIN(RLRZ3)  *  DSIN(PTRY3)  *  DCOS(YWRX3) 

DCOS (RLRZ3 )  *  DSIN(YWRX3) 

MAT3R(1,3)  =  -DSIN(PTRY3) 

MAT3R(2,3)  =  DCOS(PTRY3)  *  DSIN(YWRX3) 

MAT3R(3 ,3)  =  DCOS(PTRY3)  *DCOS(YWRX3) 

GET  THE  VELOCITIES  OF  LINK  THREE  IN  BODY  FIXED  COORDINATE  SYSTEM 

DO  609  J  -  1,3 
SUM1  =  O.ODO 
DO  610  K  =  1,3 

SUM1  =  SUM1  +  MAT3R(J,K)  *  W3(K) 

610  CONTINUE 

BRATE3( J)  =  SUM1 
609  CONTINUE 

TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  NON -ORTHOGONAL  EULER 
COORDINATE  SYSTEM  FOR  LINK  THREE 
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MAT3T (1,1)  =  O.ODO 
MAT3T( 2 , 1 )  =  l.ODO 
MAT3T(3, 1)  =  O.ODO 

MAT3T(1 ,2)  =  DCOS(YWRX3) 

MAT3T(2,2)  =  DTAN(PTRY3)  *  DSIN(YWRX3) 

MAT3T(3 ,2)  =  1 .ODO/DCOS(PTRY3)  *  DSIN(YWRX3) 

MAT3T (1,3)  =  -DSIN(YWRX3) 

MAT3T(2 , 3 )  =  DTAN(PTRY3 )  *  DC0S(YWRX3) 

MAT3T(3,3)  =  1 . ODO/DCOS (PTRY3 )  *  DCOS(YWRX3) 

*  GET  THE  VELOCITIES  OF  LINK  THREE  IN  THE  EULER  COORDINATE  SYSTEM 

DO  709  J  =  1,3 
SUM1  =  O.ODO 
DO  710  K  =  1,3 

SUM1  =  SUM1  +  MAT3T(J,K)  *  BRATE3 (K) 

710  CONTINUE 

RATE3( J)  =  SUM1 
709  CONTINUE 

RATE3X  =  RATE3 ( 1 ) 

RATE3Y  =  RATE3(2 
RATE3Z  =  RATE3(3) 

*  INTEGRATION  OF  THE  VELOCITIES  OF  LINK  THREE  IN  EULER  COOR.  SYSTEM 

YWRX3  =  INTGRLfO . , RATE3X) 

PTRY3  =  INTGRL(0. ,RATE3Y) 

RLRZ3  =  lMTGRL(-PI/2. ,RATE3Z) 

*  CONVERT  THE  ANGLES  TO  DEGREES 

YAWANX(3)  =  YWRX3  *  RADEG 

PTCANY(3)  =  PTRY3  *  RADEG 

R0LANZ(3)  =  RLRZ3  *  RADEG 

*  GET  THE  DIRECTION  COSINES  FOR  THE  LINK  THREE 

DRCSY ( 3 )  =  DCOS (RLRZ3 )  *  DSIN(PTRY3)  *  DSIN(YWRX3)  -... 

DSIN (RLRZ3 )  *  DC0S(YWRX3) 

DRCSX(3)  =  DSIN(RLRZ3)  *  DSIN(PTRY3)  *  DSIN(YWRX3)  +... 

DCOS (RLRZ3 )  *  DC0S(YWRX3) 

DRCSZ ( 3 )  =  DCOS (PTRY3 )  *  DSIN(YWRX3) 

DRCRAX( 3 )  =  DAC0S(DRCSX(3) ) 

DRCRAY ( 3 )  =  DACOS (DRCSY (  3 ) ) 

DRCRAZ( 3 )  =  DAC0S(DRCSZ(3) ) 

DP.CANX ( 3 )  =  DAC0S(DRCSX(3) )  *  RADEG 
DRCANY ( 3 )  =  DACOS  DRCSY  3)  *  RADEG 

DRCANZ(3)  =  DACOS (DRCSZ (3 ) )  *  RADEG 

JX2  =  JX1  +  (L(2,l)  +  L ( 2 , 2) )  *  DC0S(DRCRAX(2) ) 

JY2  =  JY1  +  (L  2,1  +  L  2,2  *  DCOS  DRCRAY(2  ) 

JZ2  =  JZ1  +  (L(2,l)  +  L( 2 , 2 ) )  *  DCOS ( DRCRAZ ( 2 ) ) 

TIPX  =  JX2  +  ( L ( 3 , 1 )  +  L(3 , 2 ) )  *  DC0S(DRCRAX(3) ) 

TIPY  =  JY2  +  L  3,1  +  L  3,2  *  DCOS  DRCRAY(3 

TIPZ  =  JZ2  +  (L(3,l)  +  L(3,2))  *  DCOS (DRCRAZ (3 ) ) 

DYNAMIC 

NOSORT 

*  INPUT  TORQUE  AT  JOINTS 

TOZ  -  A  *  SIN  (W  *  TIME  +  P) 

T1X  =-10  *  SIN  W  *  TIME  +  P 

T2X  =  A  *  SIN  (W  *  TIME  +  P) 

END 

STOP 

FORTRAN 
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SUBROUTINE  iO  COMMUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 


SUBROUTINE  CPROD ( VECTA , VECTB , MI , MJ ,MK) 

IMPLICIT  REAL*8  (A-Z) 

DIMENSION  VECTA(3)/VECTB(3) 

MI  *  VECTA(2)  *  VECTB (3)  -  VECTA(3)  *  VECTB (2) 

MJ  *  VECTA  3)  *  VECTB  1)  -  VECTA  1)  *  VECTB(3) 

MK  =  VECTA(l)  *  VECTB(2)  -  VECTA(2)  *  VECTB(l) 

RETURN 
END 


APPENDIX  C 


THREE  DIMENSIONAL  SIMULATION  PROGRAM 
INVESTIGATION  OF  SINGULAR  CONFIGURATION 


TERMINAL 
METHOD  ADAMS 

PRINT  .05, ERROR, ANG12Z,ANG23Z 

CONTROL  FINTIM  =4.0,  DELMAX  =.l,  DELPRT  =  .05 

SAVE  .05,  ERROR, ANG1 2X ,ANG12Y.ANG1 2Z,THETAB ,THETAD,ANG23X,ANG23Y _ 

ANG23Z , IYYT ( 2 ) , IXXT(2) , IZZT(2) 

TTME,ANG12X 
TIME , ANG12Y 
TIME , ANG12Z 
TIME , ANG23X 
TIME , ANG23Y 
TIME , ANG23Z 
TIME ,THETAB 

TIME , IYYT (2) , IXXT(2) , IZZT(2) 


GRAPH (DE=TEK6 18 
GRAPH (DE=TEK6 18 
GRAPH(DE=TEK618 
DE=TEK618 
DE=TEK618 
DE=TEK618 
DE=TEK618 
DE=TEK618 


GRAPH 
GRAPH 
GRAPH 
GRAPH 
GRAPH 
D 
D 
D 
D 
D 

EXCLUDE 
ARRAY 
ARRAY 
ARRAY 
ARRAY 
ARRAY 
ARRAY 
ARRAY 
ARRAY 
ARRAY 
ARRAY 


DIMENSION  MATA(27.27) , MASS (3, 2) ,L(3,2) , RX(3 , 2) ,RY(3 , 2 
DIMENSION  IXX( 3 , 2 ) , IXZ (3,2) ,IXY(3 ,2) , IYY(3 ,2) , IYZ(3 ,2 
DIMENSION  MAT1R  3,3) ,MAT2R(3,3) ,MAT3R(3,3' 

1) ,MAT2T(3,3) ,MAT3T(3,3 


DIMENSION  MAT1T (3,3 
INTEGER  IER , I , J ,M,K ,P,N , IA, IDGT , A 


■1! 


!: 


RZ ( 3 , 2 ) 
IZZ(3 ,2) 


-  IA, IDGT ,IER,I,J,M,K,P,N,A 
MATB(27).LCOGX(3) ,LCOGY(3) ,LCOGZ(3) 

VECTAO ( 3 )  , VECTBO ( 3 ) , VECTA1 ( 3 ) ,VECTB1(3) ,VECTA2(3) ,VECTB2(3) 
WDX(3 ) ,WDY(3) ,WDZ(3) ,W1(3) ,W2(3) ,W3(3) ,MATC(27 ) ,DQ(27 ) 

RATE1(3) , RATE2 (3 ) ,RATE3(3  , BRATE1 (3) , BRATE2(3) ,BRATE3(3) 

RBG1 ( 3 ) , RAG1 ( 3 ) , RBG2 ( 3 ) , RAG2 ( 3 ) , RBG3 ( 3 ) 

SUMHDX(3) ,SUMHDY(3) ,SUMHDZ(3) ,HDX(2) ,HDY(2) ,HDZ(2) ,WKAREA(850) 
IXXT(3 ) , IYYT(3 ) , IZZT (3) , IXYT(3) , IXZT(3) , IYZT(3) 


YAHANX 

DRCANX 

DRCRAX 


,  PTCANY ( 
,  DRCANY ( 
,  DRCRAY ( 


,ROLANZ< 
, DRCANZ 
, DRCRAZ ( 


ARRAY  DRCSX ( 3 ) , DRCSY ( 3 ) , DRCSZ ( 3 ) 
D  DATA  MATA/729  *  O.ODO/ 


INITIAL 

*  INPUT  PARAMETER  CONSTANTS 

A  =  3.0D0 
P  =  O.ODO 
W  =  PI  /  2.0D0 
IDGT  =  3 
G=Q . 0D0 
N=27 
M=1 
IA  =27 

*  INPUT  JOINT  LOCATIONS  IN  METERS 

JXO  =  O.ODO 
JYO  =  O.ODO 
JZO  =  O.ODO 
JX1  =  O.ODO 
JY1  =  O.ODO 
JZ1  =  1.0D0 

*  USE  THE  NEXT  SET  OF  JOINT  TWO  COORDINATES  FOR  CASE  A 

JX2  =  O.ODO 
JY2  =  1.0D0 
JZ2  =  1.0D0 

*  USE  THE  NEXT  SET  OF  JOINT  TWO  COORDINATES  FOR  CASE  B 
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k 

k 

k 
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JX2  =  1.0D0 
JY2  =  O.ODO 
JZ2  =  l.ODO 

USE  THE  NEXT  SET  OF  JOINT  TWO  COORDINATES  FOR  CASE  C 

JX2  =  O.ODO 
JY2  =  O.ODO 
JZ2  =  2.0D0 

INPUT  DISTANCE  FROM  CENTER  OF  LINK  TO  CENTER  OF  MASS  FOR 
EACH  LINK  ENDS 


1,1 

=  0.50D0 

1,2 

=  0.50D0 

2,1 

=  0.50DO 

2,2 

=  O.5OD0 

3,1 

=  0.50D0 

3,2 

=  0.50D0 

INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 


MASS  < 

1,1 

=  2.5D0 

MASS  ( 

1,2 

=  2.5D0 

MASS  ( 

2,1 

=  2.5D0 

MASS  ( 

2,2 

=  2.5D0 

MASS  ( 

3,1 

=  2.5D0 

MASS  { 

:3,2! 

=  2.SD0 

INPUT  OMEGA  AND  OMEGA  DOT 


DO  30  I  =  1,3 

W1(I) 

= 

O.ODO 

W2(I 

= 

O.ODO 

W3  ( I ) 

O.ODO 

WDX(I) 

= 

O.ODO 

WDY(I) 

= 

O.ODO 

WDZ(I) 

= 

O.ODO 

CONTINUE 


INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 
LINK  ONE 

LCOGX(l)  =  O.ODO 
XI  =  LCOGX(I) 

LCOGY(I)  ~  O.ODO 
Y1  =  LCOGY(l) 

LCOGZ(l)  =  0.5D0 
21  =  LCOGZ(l) 

NEXT  SET  FOR  LINK  TWO  AND  THREE  TO  USE  FOR  CASE  A 

LC0GX(2)  =  O.ODO 
X2  =  LC0GX(2 ) 

LC0GY(2)  =  0.5D0 
Y2  =  LC0GY(2) 

LC0GZ{2)  =  l.ODO 
Z2  =  LC0GZ(2) 

LC0GX( 3 )  =  O.ODO 
X3  =  LC0GX(3 ) 

LCOGY ( 3 )  =  1.5D0 
Y3  =  LCOGY ( 3 ) 

LC0GZ(3)  =  l.ODO 
Z3  =  LC0GZ(3) 

NEXT  SET  FOR  LINK  TWO  AND  THREE  TO  USE  FOR  CASE  B 

LC0GX(2)  =  0.5D0 
X2  =  LCOGX( 2 ) 

LCOGY ( 2 )  =  O.ODO 
Y2  =  LCOGY ( 2 ) 

LC0GZ(2)  =  l.ODO 
Z2  =  LC0GZ(2 ) 

LC0GX(3)  =  1.5D0 
X3  =  LC0GX(3 ) 
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LC0GY{3)  =  0.0D0 
Y3  =  LCOGY ( 3 ) 

LC0GZ(3)  =  l.ODO 
Z3  =  LC0GZ(3) 

NEXT  SET  FOR  LINK  TWO  AND  THREE  TO  USE  FOR  CASE  C 

LC0GX(2)  =  O.ODO 
X2  =  LC0GX(2 ) 

LCOGY (2)  =  O.ODO 
Y2  =  LCOGY (2) 

LCOGZ ( 2 )  =  1.5D0 
Z2  =  LCOGZ (2) 

LC0GX(3 )  =  O.ODO 
X3  =  LC0GX(3 ) 

LC0GY(3 )  =  O.ODO 
Y3  =  LCOGY? 3) 

LCOGZ (3)  =  2.5D0 
Z3  =  LC0GZ(3) 

INPUT  MASS  OF  EACH  LINK  IN  KG  AND  COMPUTE  WEIGHTS  IN  NEWTONS 

MASS1  =  5.0D0 
MASS2  =  5.0D0 
MASS3  =  5.0D0 

WG1  =  MASSING 
WG2  =  MASS2*G 
WG3  =  MASS3*G 

INPUT  ACCELERATION  OF  JOINT  ZERO 

AOX  =  O.ODO 
AOY  =  O.ODO 
AOZ  =  O.ODO 

INITIALIZE  MATRIX  A  AND  B  TO  ZERO 

DO  40  I  =  1,27 
DO  50  J  =  1,27 

MATA ( I , J )  =  O.ODO 
DQ(I)  =  O.ODO 
MATC(I)  =  O.ODO 
CONTINUE 
CONTINUE 

DO  60  I  =  1,27 
MATB(I)  =  O.ODO 
CONTINUE 

INITIALIZE  THE  INITIAL  VELOCITIES  AND  TRANSFORMATION  MATRICIES 

DO  63  I  =  1,3 
DO  64  J  =  1,3 

RATEl(I)  =  O.ODO 
RATE2 (I )  =  O.ODO 

RATE3(I)  =  O.ODO 
BRATEl(I)  =  O.ODO 
BRATE2  I  =  O.ODO 
BRATE3(I)  =  O.ODO 

MAT1T  (I,J)  =  O.ODO 
MAT2T  (I,J  =  O.ODO 
MAT3T  (I,J)  =  O.ODO 
MAT1R  I,J  =  O.ODO 
MAT2R  (I,J)  =  O.ODO 
MAT3R  (I,J)  =  O.ODO 

CONTINUE 

CONTINUE 


DERIVATIVE 

NOSORT 

CALL  ERRSET  (208,256,-1,1,1) 
LEVELQ  =  0 

CALL  UERSET( LEVELQ, LEVLDQ) 
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INITIALIZE  MATRIX  A  AND  B  TO  ZERO 

DO  70  I  =  1,27 
DO  80  J  =  1,27 

MATA ( I , J )  =  0.0D0 
CONTINUE 
CONTINUE 

DO  90  I  a  1,27 
MATB(I)  =  0 , 0D0 
DQ( I )  =  0.0D0 
CONTINUE 

INPUT  JOINT  EQUATIONS 
JOINT  ZERO  EQUATIONS 

AB  =  AG1  +  (WD1  X  RB/G1 )  +  W1  X  (W1  X  RB/G1 ) 

VECTAO (1 )  =  WDX(l) 

VECTAO  2)  =  WDY(l) 

VECTAO (3 )  =  WDZ(l) 

RBG1(1)  =  JXO  -  LCOGX(l) 

R3G1 (2 )  =  JYO  -  LCOGY(l) 

RBG1 (3 )  =  JZO  -  LCOGZ(l) 

CALL  CPROD (VECTAO , RBG1 , MIAO , MJAO , MKAO ) 

VECTAO (1 )  =  Wl(l) 

VECTAO (2)  =  Wl(2 
VECTAO (3)  =  Wl(3) 

CALL  CPROD ( VECTAO , RBG1 , MI BO , MJBO , MKBO ) 

VECTBO ( 1 )  =  MIBO 
VECTBO (2)  =  MJBO 
VECTBO (3)  =  MKBO 

CALL  CPROD (VECTAO , VECTBO , MI CO , MJCO , MKCO ) 
JOINT  ONE  EQUATIONS  — - 

AA  =  AG1  +  (WD1  X  RA/G1)  +  W1  X  (W1  X  RA/G1 ) 

VECTA1 ( 1 )  =  WDX(l) 

VECTA1 (2)  =  WDY  l) 

VECTA1 (3 )  =  WDZ(l) 

RAG1(1)  =  JX1  -  LCOGX(l) 

RAG1 (2 )  =  JY1  -  LCOGY(l) 

RAG1 ( 3 )  =  JZ1  -  LCOGZ(l) 

CALL  CPROD (VECTA1 ,RAG1 , MI A1 ,MJA1 ,MKA1) 

VECTAl(l)  =  Wl(l) 

VECTA1 (2)  =  Wl(2 
VECTA1(3)  =  Wl(3) 

CALL  CPROD  (VECTA1 ,RAG1 ,MIB1 ,MJB1 ,MKB1) 

VECTB1 ( 1 )  =  MIB1 
VECTB1  2}  =  MJB1 
VECTB1 (3 )  =  MKB1 

CALL  CPROD  (VECTA1 , VECTB1 ,MIC1 ,MJC1 ,MKC1 ) 
AB  =  AG2  +  (WD2  X  RB/G2)  +  W2  X  (W2  X  RB/G2) 

VECTAl(l)  =  WDX(2 ) 

VECTA1 (2 )  =  WDY (2 ) 

VECTA1 (3)  =  WDZ(2) 

RBG2 ( 1 )  =  JX1  -  LC0GX(2) 

RBG2  2)  =  JY1  -  LCOGY ( 2 ) 

RBG2 (3)  =  JZ1  -  LC0GZ(2) 

CALL  CPROD  (VECTA1 ,RBG2 ,MIA2 ,MJA2 ,HKA2) 

VECTA1 ( 1 )  =  W2 ( 1 ) 

VECTA1 (2 )  =  W2(2) 


VECTA1 (3 )  =  W2(3) 

CALL  CPROD  (VECTA1 ,RBG2 ,MIB2 ,MJB2 ,MKB2) 

VECTB1 (1 )  =  MIB2 
VECTB1 (2)  =  MJB2 
VECTB1 (3)  =  MKB2 

CALL  CPROD  (VECTA1 , VECTB1 ,MIC2 ,MJC2 ,MKC2) 


★ 

* 


* 


* 


* 


JOINT  TWO  EQUATIONS 

AA  =  AG2  +  (WD2  X  RA/G2)  +  W2  X  (W2  X  RA/G2 ) 

VECTA2 ( 1 )  =  WDX(2) 

VECTA2(2  =  WDY ( 2 ) 

VECTA2 (3 )  =  WDZ ( 2 ) 

RAG2(1)  =  JX2  -  LCOGX(2) 

RAG2<2)  =  JY2  -  LCOGY  2 
RAG2(3)  =  JZ2  -  LCOGZ (2 ) 

CALL  CPROD  (VECTA2 , RAG2 ,MIA3 ,MJA3 (MKA3 ) 

VECTA2 { 1 )  =  W2 ( 1 ) 

VECTA2 (2 )  =  W2  2) 

VECTA2 (3 )  =  W2(3) 

CALL  CPROD  (VECTA2 , RAG2 ,MIB3 ,MJB3 ,MKB3) 

VECTB2 ( 1 )  =  MIB3 
VECTB2  2  =  MJB3 

VECTB2 (3 )  =  MKB3 

CALL  CPROD ( VECTA2 , VECTB2 , MIC3 , MJC3 , MKC3 ) 
AB  =  AG3  +  (WD3  X  RB/G3)  +  W3  X  (W3  X  RB/G3) 

VECTA2 (1 )  =  WDX(3) 

VECTA2(2  =  WDY  3 
VECTA2(3)  =  WDZ(3) 

RBG3 ( 1 )  =  JX2  >  LCOGX(3) 

RBG3 ( 2 )  =  JY2  -  LCOGY (3) 

RBG3 (3 )  =  JZ2  -  LCOGZ(3) 

CALL  CPROD  ( VECTA2 , RBG3 , MI A4 , MKA4 , MKA4 ) 

VECTA2(1)  =  W3 ( 1 ) 

VECTA2(2)  =  W3  2 
VECTA2 (3 )  =  W3(3) 

CALL  CPROD  (VECTA2 , RBG3 , MIB4 ,MJB4 ,MKB4) 

VECTB2(1)  =  MIB4 
VECTB2  2  =  MJB4 

VECTB2 (3 )  =  MKB4 

CALL  CPROD  (VECTA2 , VECTB2 ,MIC4 ,MJC4,MKC4) 
SUM  OF  MOMENTS  EQUATIONS 
DO  100  I  =  1,3 


COMPUTE  HX , HDOT  X,HY,HDOT  Y,  HZ ,HDOT  Z 


RX 

'I  1 

=  -L 

1,1] 

*  DCOS 

'DRCRAX 

I, 

) 

RX 

I  2 

=  L 

1,2 

*  DCOS 

DRCRAX 

I{ 

RY 

I  1 

=  -L 

1,1 

*  DCOS 

,DRCRAY 

.1 

) 

RY 

T  2 

=  L 

1,2 

*  DCOS 

DRCRAY 

T 

> 

RZ 

T  1 

x  t  «*■ 

=  -L 

1,1, 

*  DCOS 

DRCRAZ 

T 

} 

RZ 

T  2 

=  L 

1,2 

*  DCOS 

DRCRAZ 

T' 

1 

) 

IXX(I.l)  =  MASS 
IXX( I ,2)  =  MASS 
IXXT(I)  =  IXX(l.l)  + 

IF  (IXXT(I)  .LE.  .020)  THEN 
IXXT(I)  =  .020 
ELSE 

IXXT(I)  =  IXXT(I) 

END  IF 


IXX(I ,2) 


*  RY 

*  RY 


+ 

+ 


*  RZ(  I 

*  RZ(I 
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IXY(I,1)  =  MASS (1,1)  *  RX ( I , 1 ) 
iml,2  =  MASSII ,2)  *  RX ( 1 , 2  j 
IXYT(I)  =  IXJ(I.l)  +  IXY(I ,2) 

IXZ(I.l)  *  MASS (I , 1 )  *  RZ ( I , 1 ) 
IXZ(1 ,2)  =  MASS (1 ,2)  *  RZ ( I , 2 i 
IXZT(I)  =  IXZ(I,1)  +  IXZ(I , 2) 


*  RY 

*  RY 


(i:ij 


*  RX ( I , 1 

*  RX( I , 2 


HDX(1 

)  =  WDX( 1 

)  *  IXX(I,1)-  WDZ ( I 

)  *  IXZ(I,1) 

-  WDY ( I 

}  t 

HDX(2 

)  =  WDX(2 

)  *  IXX(I ,2)-  WDZ (I 

)  *  IXZ(I ,2) 

-  WDY (I 

)  * 

IYY (1,1)  =  MASS (1,1)  , ,  , 

IYY(I ,2)  =  MASS (I , 2 )  *  ({RX(l 
IYYT(I)  =  IYY (1,1)  +  IYY (1,2) 
IF  (IYYT(I)  .LE.  .020)  THEN 
IYYT(I)  =  .020 
ELSE 

IYYT(I)  =  IYYT(I) 

END  IF 


RX 

RX 


ii:i»  J 


(RZ(I,1 
(RZ(1 ,2 


IYZ(I , 1 ) 

=  MASS ( 

1,1) 

*  RY  1 

1,1) 

*  RZ(I,1) 

IYZ(I ,2) 

*  MASS 

1,2 

*  RYI 

1,2 

*  RZ ( I , 2 ) 

IYZT(I) 

=  IYZ(  I 

:,i)  +  iyzi 

1,2 

IXY (1,1) 
IXY( 1,2) 


*  RZ 

*  RZ 


)  =  WDY ( I 

)  *  IYY 

Xi) 

-  WDX( I 

)  *  IXY< 

1 , 1) 

-  WDZ (I 

)  *  IYZ( 

)  =  WDY (I 

)  *  IYY 

il  ,2) 

-  WDX(I 

)  *  IXY 

1,2) 

-  WDZ (I 

)  *  IYZ( 

HDY(2 

IZZ(I,1)  =  MASS (I ,1 
IZZ(I ,2)  =  MASS (I, 2 


)  *  ( (RX( I , 1 ) 

*  RX( 

:i,iy 

)  +  (RY< 

I1-1) 

*  RYI 

XI)) 

)  *  ((RX(I.2) 

*  RX( 

!i,2): 

)  +  (ry! 

!l,2) 

*  RYI 

:i,2)) 
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★ 

* 


IZZT(I)  *  .020 
ELSE 

IZZT(I)  =  IZZT(I) 
END  IF 


(1)  =  WDZ (I 

)  *  IZZI 

;  i , 1 ) 

-  WDX(I 

)  *  IXZI 

[1.1)  - 

(2)  =  WDZ ( I 

)  *  IZZI 

[1.2) 

-  WDX(I 

)  *  IXZI 

!l,2)  - 

-  WDY  ( 


I)  *  IYZlI , 1 ) 
I)  *  IYZ(I ,2) 


SUMHDX( 
SUMHDY 
SUMHDZ ( 


=  HDX( 1 
=  HDY(1 
=  HDZ( 1 


CONTINUE 

ENTER  CONSTANTS  INTO  MATRIX  A 
LINK  ONE 

SUM  OF  FORCES  IN  THE  X  DIRECTION 


MATA ( 1 , 1 )  = 

MATAil ,4)  = 


1.0D0 

MASS1 


MATA ( 1 , 10 )  =  -1 . 0D0 


SUM  OF  FORCES  IN  Y  DIRECTION 

MATA (2 , 2 )  =  1.0D0 

MATA( 2 , 5 )  =  MASS1 

MATA(2, 11)  =  -1.0D0 

SUM  OF  FORCES  IN  Z  DIRECTION 


MATA (3 , 3 )  = 

MATA (3 , 6 )  = 


1.0D0 
MAS  SI 


MATA(3 , 12)  =  -1.0D0 
EQUATIONS  AT  JOINT  ZERO 
IN  THE  X  DIRECTION 

MATA(4,4)  =  1.0D0 

MATA (4 , 8)  =  RBG1 (3 ) 
MATA (4,9)  =  -RBG1 (2) 

IN  THE  Y  DIRECTION 

MATA (5 ,5)  =  1.0D0 

MATA (5 ,7 }  =  -RBG1 (3 
MATA (5 ,9)  =  RBG1 (l 
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* 


* 


* 


* 


* 


* 


•k 


* 


* 


•k 


IN  THE  Z  DIRECTION 


NAT?*  (6,6)  =  1.QD0 

MATA (6 , 7 )  =  RBG1 (2) 
MATA (6 ,8)  =  -RBGl(l) 


SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  ONE  IN  THE  X,Y,Z  DIRECTIONS 


MATA 

7'2 

=  RBG1 

>3 

MATA 

7,3 

=  -RBG1 

MATA 

7,7 

=  -IXXT 

1 

MATA 

7,8 

=  IXYT 

1 

MATA 

7,9 

=  IXZT 

1 

MATA 

7,1] 

L)  =  -RAG1 

3 

MATA 

(7,12)  =  RAG1 

\2) 

MATA 

(8,1. 

=  -RBG1 

(3) 

MATA 

[8,3 

=  RBG1 

1 

MATA 

8,7 

=  IXYT 

1 

MATA 

8,5 

=  -IYYT 

1 

MATA 

8,9 

=  IYZT 

,1 

MATA 

^8 , 1C 

))  =  RAG1 

3 

MATA1 

(8,12)  =  -RAG1 

*1) 

MATA 

9  1 

=  RBG1 

(2) 

MATA 

9,2 , 

=  -RBG1 

1 

MATA 

9,7 

=  IXZT 

1 

MATA 

>9,8 

=  IYZT 

1 

MATA 

9,9 

=  -IZZT 

1 

MATA 

9, 1C 

))  =  -RAG1 

\2l 

MATA 

'9,11)  =  RAG1 

1 

+  IXZT(2) 
+  IYZT ( 2 ) 
-  IZZT (2) 


+  IXZT(3) 
+  IYZT (3 ) 
-  IZZT (3) 


LINK  TWO 


SUM  OF  FORCES  IN  X  DIRECTION 

MATA ( 10 ,  10 )  =  1.0D0 

MATA( 10 , 13)  =  MASS2 
MATA(10,19)  =  -1.0D0 

SUM  OF  FORCES  IN  THE  Y  DIRECTION 

MATA (11,11)  =  1.0D0 

MATA( 11 , 14 )  =  MASS 2 
MATA( 11 , 20 )  =  -1.0D0 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 

MATA(12,12)  =  1.0D0 

MATA ( 12 ,  15 )  =  MASS2 
MATA ( 12 ,  21 ;  =  -1.0D0 

EQUATIONS  AT  JOINT  ONE 
IN  THE  X  DIRECTION 

MATA( 13,4)  =  -1.0D0 

MATA (13, 8)  =  -RAG1 ( 3 ) 

MATA ( 13 , 9 )  =  RAG1 ( 2 ) 

MATA( 13 , 13)  =  1.0D0 

MATA( 13,17)  =  RBG2 ( 3 ) 

MATA( 13 , 18)  =  -RBG2 ( 2 ) 

IN  THE  Y  DIRECTION 


MATA( 14 , 5 ) 

=  -1.0D0 

MATA ( 14 , 7 ) 

=  RAG1 ( 3 ) 

MATA( 14,9; 

=  -RAG1(1) 

MATA  14,14) 

=  1.0D0 

MATA( 14,16) 

=  -RBG2 (3 ) 

MATA(14 ,18) 

=  RBG2 ( 1 ) 

IN  THE  Z  DIRECTION 


MATA(15 ,6) 

= 

-1.0D0 

MATA  15,7 

sr 

-RAG1(2) 

MATA(15 ,8) 

= 

RAG1(1) 

MATA( 15,15) 

= 

1.0D0 

MATA( 15 , 16) 

= 

RBG2 ( 2 ) 
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\ 


I 

1 


* 


MATA(15,17)  =  -RBG2(1) 
SUM  OF  MOMENTS  EQUATIONS  FOR 


MATA 

16,11 

=  RBG2 

3 

MATA 

16,12 

=  -RBG2 

2 

MATA 

16,16 

=  -IXXT 

2 

MATA 

16,17 

=  IXYT 

,2 

MATA 

16,18, 

=  IXZT 

2 

MATA 

16,20 

=  -RAG2 

,3 

MATA 

1 16 , 21 

=  RAG  2 

\2) 

MATAI 

,17,10' 

=  -RBG2 

MATA 

17,12 

=  RBG2 

1 

MATAI 

17,16 

=  IXYT 

2 

MATA 

17,17 

=  -IYYT 

2 

MATA 

17 , 18 

=  IYZT 

2 

MATAI 

17,19 

=  RAG2 

3 

MATAI 

17 ,21 j 

=  -RAG 2 

kl) 

MATAI 

18,10] 

=  RBG2 

'2) 

MATA 

18,11 

=  -RBG2 

MATA 

18,16, 

=  IXZT 

2 

MATAI 

18,17 

=  IYZT 

2 ) 

MATA 

18,18 

=  -IZZT 

2) 

MATAI 

18,19 

=  -RAG2 

2 

MATAI 

18,20 

=  RAG  2 

U) 

LINK  TWO  IN  THE  X,Y.Z  DIRECTIONS 


IXZT(3) 
IYZT  (3 
IZZT (3 ) 


LINK  THREE 

SUM  OF  FORCES  IN  THE  X  DIRECTION 

MATA( 19 , 19 )  =  1.0D0 

MATA (19, 22)  =  MASS3 

SUM  OF  FORCES  IN  THE  Y  DIRECTION 


!IATA(  20 , 20 )  =  1.0D0 

MATA(20 , 23 )  =  MASS3 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 

MATA(21 ,21)  =  1.0D0 

MATA(21,24)  =  KASS3 

EQUATIONS  AT  JOINT  TWO 
IN  THE  X  DIRECTION 


MATAI 

22,13 

=  -l.ODC 

) 

MATAI 

22,17 

=  -RAG 2 1 

3) 

MATAI 

22,18 

=  RAG 2 ( 

2 

MATAI 

MATAI 

22,22 

22,26 

=  l.ODC 
=  RBG3I 

) 

;3) 

MATAI 

[22,27; 

=  -RBG3I 

2 

IN  THE  Y  DIRECTION 

MATA(23 , 14)  =  -1.0D0 
MATAI 23 , 16 )  =  RAG2(3) 

MATAi 23 ,18)  =  -RAG2(l) 

MATA ( 23 , 23 )  =  1.0D0 

MATAi 23 , 25 )  =  -RBG3(3) 

MATA(23 , 27 )  =  RBG3(l) 

IN  THE  Z  DIRECTION 

MATA(24 , 15 )  =  -1.0D0 
MATA( 24 , 16 )  =  -RAG2(2) 

MATA  24,17)  =  RAG2(l) 

MATA  24,24)  =  1.0D0 

MATA (24 , 25 )  =  RBG3(2) 

MATA(  24 , 26 )  =  -RBG3U) 

SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  THREE  IN  THE  X,Y,Z  DIRECTIONS 

MATA (25, 20)  =  RBG3(3) 

MATAi 25 , 21 )  =  -RBG3(2 
MATA(25,25  =  -IXXT(3 
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* 


MATAl 

[25,26) 

= 

iXYi 

MATAi 

[25,27) 

= 

IXZT 

MATAl 

[26 , 19) 

= 

-RBG3 

MATAI 

26,21 

= 

RBG3 

MATAl 

26,25 

S 

IXYT 

MATAI 

26,26 

S 

-IYYT 

MATAI 

[26,27) 

= 

IYZT 

MATAI 

[27,19) 

s 

RBG3 

MATAI 

27,20 

= 

-RBG3 

MATAI 

27,25 

= 

IXZT 

MATAl 

27,26 

= 

IYZT 

MATAI 

27,27) 

= 

-IZZT 

(2) 

(1) 

(3) 

(3) 

(3) 


GO  TO  1112 


*  INITIALIZE  MATRIX  ACCORDING  TO  CONSTRAINT 

*  CONSTRAINTS  GROUP  1  WHEN  ONLY  LINK  THREE  IS  IN  MOTION 

*  DO  118  I  =  1,18 

*  DO  18  J  =  1,27 

*  MATA { I , J )  =  0.0 

*  MATA (1,1)  =1.0 

*  MATB(I)  =0.0 

*18  CONTINUE 

*118  CONTINUE 

: k 

*  DO  181  I  =  19,27 

*  DO  81  J  =  1,18 

*  MATA ( I , J )  =  0.0 

*81  CONTINUE 

*181  CONTINUE 

*  GO  TO  1111 


CONSTRAINTS  GROUP  2  WHEN  LINK  TWO  AND  THREE  ARE  IN  MOTION 
DO  19  I  =  1,9 


DO  191  J  =  1,27 


k 

MATA 1 1 ,  J }  = 

O.ODO 

•k 

MATA(I.l)  = 

1.0  DO 

k 

k 

MATB(l)  = 

O.ODO 

k 

MATAI17 , J) 

=  O.ODO 

k 

MATAI 1 8, j) 

=  O.ODO 

k 

MATB ( 17 ) 

=  O.ODO 

k 

MATB (18) 

=  0.D0 

k 

k 

MATAl J, 17 ) 

=  O.ODO 

k 

MATA(J,18) 

=  O.ODO 

k 

k 

MATAl 17, 17) 

=  1.0D0 

k 

k 

MATA( 18,18) 

=  1.0D0 

*191 

CONTINUE 

*19 

k 

CONTINUE 

k 

DO  91  I  =  10,27 

k 

DO  92  J  =  1,9 

k 

MATA ( I , J )  = 

0.0 

*92 

CONTINUE 

*91 

CONTINUE 

* 

GO  TO  1111 

CONSTRAINTS  GROUP  3  WHEN  THREE  OF  THE  LINKS  ARE  IN  MOTION 


DO  78  J  =  1,27 


k 

MATA 17 , J )  = 

O.ODO 

k 

MATA  8, J  = 

O.ODO 

k 

MATAI J, 7  = 

O.ODO 

k 

MATAl J , 8;  = 

O.ODO 

k 

MATB (7) 

O.ODO 

k 

MATB(8) 

O.ODO 

91 


*  *  XX  XX  X  X  X  X  X 


LINEAR  VELOCITIES 

MATB (24)  =  MATB(15)+(THDDOT*RAG2(2))-(THTDOT**2)*(RAG2(3)) 
MATB  23  =  MATB  14  -(THDDOT*RAG2(3))-(THTDOT**2)*(RAG2(2)) 
MATB (22;  =  MATB (13) 

END  OF  THE  INFORMATION  FOR  X  DIRECTION 


*  USE  THE  FOLLOWING  SET  OF  INFORMATION  WHEN  THE  ANGULAR  VELOCITY  IS 

*  IN  THE  Y  DIRECTION  REGARDLESS  OF  THE  INITIAL  CONFIGURATION 

* 

*  ENTER  THE  THEORITICAL  VALUES  ASSUMING  THE  LINK  TWO  AND  THREE  ARE 

*  IN  PLANAR  MOTION  AND  ANGULAR  VELOCITY  IS  IN  THE  Y  DIRECTION 


* 

* 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 


LINK  TWO 

THEORITICAL  ANGULAR  VELOCITIES (APPLIED  IN  THE  Y  DIRECTION  ) 


MATB (18)  = 

MATB (17)  = 

MATB (16)  = 

THDDOT 

THTDOT 

THETRB 

THETAB 

LINEAR  VELOCITIES 


0.0D0 

((-PI**3)/8)*SIN(W*TIME) 

O.ODO 
MATB (17) 

INTGRL ( (PI**2)/4. , THDDOT) 

INTGRL(0 , INTGRL(PI**2/4 . , THDDOT) ) 
THETRB  *  RADEG 


MATB (15)  =  (THDDOT  *  RBG2(1))  +  (THTDOT  **  2) 
MATB (14)  =  O.ODO 

MATB (13)  =  -(THDDOT  *  RBG2(3))  +  (THTDOT  **  2) 

LINK  THREE 
ANGULAR  VELOCITIES 


*  RBG2(3) 

*  RBG2 ( 1 ) 


*  MATB (27 )  =  O.ODO 

*  MATB (26)  =  O.ODO 

*  MATB (25)  =  O.ODO 

*  LINEAR  VELOCITIES 


*  MATB (24)  =  MATB(15)-(THDDOT*RAG2(l))-(THTDOT**2)*(RAG2(3)) 

*  MATB  23)  =  MATB ( 14) 

*  MATB (22)  =  MATB ( 13 )+(THDDOT*RAG2 (3) ) - (THTDOT**2)*(RAG2 ( 1 ) ) 

*  END  OF  THE  INFORMATION  FOR  THE  Y  DIRECTION 


*  USE  THE  FOLLOWING  SET  OF  INFORMATION  WHEN  THE  ANGULAR  VELOCITY  IS 

*  IN  THE  Z  DIRECTION  REGARDLESS  OF  THE  INITIAL  CONFIGURATION 

*  ENTER  THE  THEORITICAL  VALUES  ASSUMING  THE  LINK  TWO  AND  THREE  ARE 

*  IN  PLANAR  MOTION  AND  ANGULAR  VELOCITY  IS  IN  THE  Z  DIRECTION 

*  LINK  TWO 

*  THEORITICAL  ANGULAR  VELOCITIES (APPLIED  IN  THE  Z  DIRECTION  ) 

*  MATB (16)  =  O.ODO 

*  MATB (17 )  =  O.ODO 

*  MATB ( 18)  =  - ( (PI**3 )  /  8.0D0)  *  DSIN(W  *  TIME) 

*  THDDOT  =  MATB (18) 

*  THTDOT  =  INTGRL ( (PI**2 )/4 . .THDDOT) 

*  THETRB  =  INTGRL (0. .THTDOT) 

*  THETAB  =  THETRB  *  RADEG 

*  LINEAR  VELOCITIES 

*  MATB (14)  =  -(THDDOT  *  RBG2(1))  +  (THTDOT  **  2)  *  RBG2(2) 

*  MATB (13 )  =  (THDDOT  *  RBG2(2))  +  (THTDOT  **  2)  *  RBG2(l) 

*  MATB(15)  =  O.ODO 

*  LINK  THREE 

*  ANGULAR  VELOCITIES 

* 

*  MATB (27)  =  O.ODO 

*  MATB ( 26)  =  O.ODO 

*  MATB (25)  =  O.ODO 

*  LINEAR  VELOCITIES 

*  MATB (24)  =  MATB (15) 

*  MATB (23 )  =  MATB  14  +(THDD0T*RAG2(1) )-(THTDOT**2)*(RAG2(2) ) 

*  MATB(22)  =  MATB(13)-(THDD0T*RAG2(2) )-(THTDOT**2)*(RAG2(l) ) 

*  END  OF  THE  INFORMATION  FOR  THE  Z  DIRECTION 

*  NEXT  SET  OF  STATEMENTS  ARE  COMMON  IN  ANY  PLANAR  MOTION  AND  THEY  ARE 

*  IN  YHE  CODE  IN  EVERY  CASE.  THESE  TERMS  ARE  ACCELERATION  OF  THE  LINK 
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*  ONE  AND  FORCES  AT  EACH  JOINT 

*  LINK  ONE  LINEAR  AND  ANGULAR  ACCELERATIONS 


★ 

★ 

* 


•k 


k 


k 


MATBl 

'4' 

\  *  l 

=  0.0D0 

MATBl 

5 

=  O.CDO 

MATBl 

6 

=  0.0D0 

MATBl 

7 

=  0.0D0 

MATBl 

8 

=  O.ODO 

MATBl 

9 

=  O.ODO 

FORCES 
JOINT  TWO 


MATB(21 )  =  -MASS3  *  MATB(24)  -  WG3 
MATB(20)  =  -MASS3  *  HATB  23) 

MATB(19)  =  -MASS3  *  MATB(2 2) 

JOINT  ONE 

MATB(12)  =  HATB (21 )  -  MASS2  *  MATB(15)  -WG2 
MATS (11)  =  MATB(20)  -  MASS2  *  MATB (14) 

MATB (10)  =  MATB (19)  -  MASS2  *  MATB (13) 

JOINT  ZERO 


MATB (3)  =  MATB (12) 
MATB (2  =  MATB (11) 
MATB ( 1 )  =  MATB(IO) 

END  OF  THE  INFORMATION 


-  MASS1  *  MATB (6) 

-  MASS1  *  MATB ( 5 ) 

-  MASS1  *  MATB (4) 


-WG1 


*  MULTIPLY  MATA  AND  MATB 


555 

505 


DO  505  J  =  1,27 
SUM1  =0.0 

DO  555  K  =  1,27 

SUM1  =  SUM1  +  MATA( J,K)  *  MATB(K) 
CONTINUE 
DQ(  J)  =  SUM1 
CONTINUE 


506 


DO  506  I  =1,27 

MATC(I)  =  DQ(I ) 
CONTINUE 


CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 
CALL  LEQT2 F ( MATA , M , N , I A , DQ , IDGT , WKAREA , IER) 
IF  (IER  .NE.  0)  CALL  END JOB 


* 

k 


FIND  LCOGX , LCOGY , LCOGZ , THETA  VALUES ,WX,WY,WZ 


LINK  ONE 

AX1 

VELX1 

LCOGX1 

LCOGX(l) 

AY1 
VELY1 
LCOGY1 
LCOGY ( 1 ) 

AZ1 
VELZ1 
LCOGZ1 
LCOGZ (1 ) 

WD1X 

W1X 

WDX(l) 

W1(1) 

WD1Y 

W1Y 

WDY(l) 

Wl(2) 


DO  (4) 

INTGRL(0 . , AX1 ) 
INTGRL(X1 ,VELX1) 
LCOGX1 

DQ(5) 

INTGRL(0 . , AY1 ) 
INTGRL(Y1 ,VELY1) 
LCOGY 1 

DO(6) 

INTGRL(0 . , AZ1 ) 
INTGRL (Z1 , VELZ1 ) 
LCOGZ1 

I$TGRL(0. , WD1X) 

WD1X 

W1X 

DO(8) 

INTGRL(0 . , WD1Y) 

WD1Y 

W1Y 


94 


#-  *  *  * 


WDiZ  -  DQ( 5) 

W1Z  =  INTGRL(0. ,WD1Z) 

WDZ(l)  =  WDIZ 

Wl(3)  =  W1Z 

TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COORDINATE 
SYSTEM  FOR  THE  LINK  ONE 

MATlR(l.l)  =  DCOS(RLRZl)  *  DCOS(PTRYl) 

MATIR(2 , 1 )  =  DCOS(RLRZL)  *  DSIN(PTRYl)  *  DSIN(YWRXl) 
DSIN(RLRZl)  *  DCOS(YWRXl) 

MAT1R(3 , 1 )  =  DCOS(RLRZl)  *  DSIN(PTRYl)  *  DCOS(YWRXl)  +... 
DSIN(RLRZl)  *  DSIN(YWRXl) 

MAT1R( 1 , 2 )  =  DSIN(RLRZl)  *  DCOS(PTRYl) 

MAT1R(2 , 2)  =  DSIN(RLRZl)  *  DSIN(PTRYl)  *  DSIN(YWRXl)  +... 
DCOS(RLRZl)  *  DCOS(YWRXl) 

MAT1R( 3 , 2 )  =  DSIN(RLRZl)  *  DSIN(PTRYl)  *  DCOS(YWRXl)  -... 
DCOS(RLRZl)  *  DSIN(YWRXl) 

MAT1R( 1 , 3 )  =  -DSIN(PTRYl) 

MAT1R(2 , 3 )  =  DCOS(PTRYl)  *  DSIN(YWRXl) 

MAT1R(3,3)  =  DCOS(PTRYI)  *  DCOS(YWRXl) 

GET  THE  VELOCITIES  FOR  LINK  1  IN  BODY  FIXED  COOR.  SYSTEM 

DO  605  J  =  1,3 
SUM1  =  0.0D0 
DO  606  K  =  1,3 

SUM1  =  SUM1  +  MAT1R(J,K)  *  W1(K) 

606  CONTINUE 

BRATEl(J)  =  SUM1 
605  CONTINUE 

TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  EULER  COORDINATE 
SYSTEM  FOR  THE  LINK  ONE 

MAT1T( 1,1)  =  O.ODO 
MAT1T(2,1)  =  1.0D0 
MAT1T(3 , 1 )  =  O.ODO 

MAT 1 T ( 1 , 2 )  =  DCOS(YWRXl) 

MAT1T (2,2)  =  DTAN(PTRYl)  *  DSIN'YWRXl) 

MAT1T(3,2)  =  1 .ODO/DCOS(PTRY1)  *  DSIN(YWRXl) 

MAT1T (1,3/  =  -DSIN(YWRXl) 

MAT1T(2 , 3 }  =  DTAN(PTRYl)  *  DCOS(YWRXl) 

MAT1T(3,3)  =  1 . DO/DCOS (PTRY1 )  *  DCOS(YWRXl) 

GET  THE  YAW, PITCH  AND  THE  ROLL  RATES  FOR  LINK  ONE 

DO  705  J  =  1,3 
SUM1  =  O.ODO 
DO  706  K  *  1,3 

SUM1  =  SUM1  +  MATIT(J.K)  *  BRACE  1  K 
706  CONTINUE 

RATEl(J)  =  SUM1 
705  CONTINUE 

RATE  IX  =  RATElfn 
RATE1Y  =  RATE  1(2') 

RATE1Z  =  RATE  1 ( 1 • 

YWRX1 
PTRY1 
RLRZ I 

VAWnMX : ; 

?  7  r  A*  .’V 
?  Z  rZ « Z 
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it 


it 
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it 

it 
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it 
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it 
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it 
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DRCSX(l)  =  DSIN(RLRZl)  *  DSIN(PTRYl)  *  DCOS(YWRXl) 
DCOS(RLRZl)  *  DSIN(YWRXl) 

DRCSZ(l)  =  DCOS(PTRYl)  *  DCOS(YWRXl) 

GET  THE  ANGLES  AS  RADIANS 

DRCRAX(l)  =  DACOS (DRCSX{1 ) ) 

DRCRAY(l)  =  DACOS (DRCSY ( 1 ) ) 

DRCRAZ(l)  =  DACOS (DRCSZ ( 1 ) ) 

CONVERT  THE  DIRECTION  COSINES  TO  DEGREES 

DRCANX ( 1 )  =  DACOS (DRCSX(l))  *  RADEG 
DRCANY ( 1 )  =  DACOS (DRCSY(l))  *  RADEG 
DRCANZ(l)  =  DACOS (DRCSZ(l))  *  RADEG 

LINK  TWO 

AX2  =  DQ(13) 

VELX2  =  INTGRL ( 0 . , AX2 ) 

LCOGX2  =  INTGRL (X2,VELX2) 

LCOGX(2)  =  LCOGX2 

AY2  =  DO (14) 

VELY2  =  INTGRL ( 0. ,AY2) 

LCOGY2  =  INTGRL (Y2,VELY2) 

LCOGY(2)  =  LCOGY2 

AZ2  =  DO (15) 

VELZ2  =  INTGRL ( 0. ,AZ2) 

LCOGZ2  =  INTGRL (Z2.VELZ2) 

LCOGZ(2)  =  LCOGZ2 

WD2X  =  DO (16) 

W2X  a  INTGRL((PI**2)/4. ,WD2X) 

USE  THE  INIT.  COND.  WITH  ONLY  WHICHEVER  VELOCITY  APPLIED 
AND  KEEP  THE  TWO  OTHER  ANG.  VEL.  INIT.  COND.  AS  ZERO 


USE  THE  NEXT  STATEMENT  IF  THE  ANGULAR  VELOCITY  IS  IN  THE  X  DIR. 


THETRD 

WDX(2) 

W2(l) 

WD2Y 

W2Y 


=  INTGRL (0.,W2X) 

*  WD2X 
=  W2X 


=  INTGRL 


GRL(0 . ,WD2Y) 


USE  THE  NEXT  STATEMENT  IF  THE  ANGULAR  VELOCITY  IS  IN  THE  Y  DIR. 


THETRD 

WDY(2) 

W2(2) 

WD2Z 

W2Z 


=  INTGRL ( 0. ,W2Y) 

=  WD2Y 
a  W2Y 


a  DO ( 1 8  ) 
=  INTGRL 


TGRL(0. ,WD2Z) 


USE  THE  NEXT  STATEMENT  IF  THE  ANGULAR  VELOCITY  IS  IN  THE  Z  DIR. 
THETRD  =  INTGRL ( 0. ,W2Z) 


THETRD  =  INTGRL ( 0. ,W2Z) 

WDZ(2)  =  WD2Z 

W2(3)  =  W2Z 

THETAD  =  THETRD  *  RADEG 

ERROR  =  ABS( ( (THETAD-THETAB) /180 . )  *  100) 

TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COORDINATE 
SYSTEM  FOR  THE  LINK  TWO 


MAT2R(1 , 1 )  =  DCOS(RLRZ2)  *  DCOS(PTRY2) 

MAT2R(2 , 1)  =  DCOS(RLRZ2)  *  DSIN(PTRY2)  *  DSIN(YWRX2)  -... 
DSIN(RLRZ2)  *  DCOS(YWRX2) 

MAT2R(3 , 1)  =  DCOS(RLRZ2)  *  DSIN(PTRY2)  *  DCOS(YWRX2)  +... 
DSIN(RLRZZ)  *  DSIN(YWRX2) 

MAT2R(1 ,2)  =  DSIN(RLRZ2)  *  DCOS(PTRY2) 

MAT2R(2 , 2)  =  DSIN(RLRZ2)  *  DSIN(PTRY2)  *  DSIN(YWRX2)  +... 
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*  * 


DC0S(RLRZ2)  *  DC0S(YWRX2) 

MAT2R(3 ,2)  =  DSIN(RLRZ2)  *  DSIN(PTRY2)  *  DC0S(YWRX2)  -... 
DCOS(RLRZz)  *  DSIN(YWRX2) 

MAT2R(1,3)  =  -DSIN(PTRY2) 

MAT2R(2,3)  =  DCOS(PTRY2)  *  DSIN(YWRX2) 

MAT2R(3,3)  =  DCOS(PTRY2)  *  DCOS(YWRX2) 

*  GET  THE  VELOCITIES  FOR  LINK  2  IN  BODY  FIXED  COOR.  SYSTEM 

DO  607  J  =  1,3 
SUM1  =  0.0D0 
DO  608  K  *  1,3 

SUM1  =  SUM1  +  MAT2R(J,K)  *  W2(K) 

608  CONTINUE 

8RATE2( J)  =  SUM1 
607  CONTINUE 

TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  EULER  COOR.  SYSTEM 
FOR  THE  LINK  TWO 

MAT2T (1,1)  =  O.ODO 
MAT2T(2,1)  =  1.000 
MAT2T(3,1)  =  O.ODO 

MAT2T (1,2)  =  DCOS (YWRX2 ) 

MAT2T(2,2)  =  DTAN(PTRY2)  *  DSIN(YWRX2) 

MAT2T(3,2)  =  1 .ODO/DCOS(PTRY2)  *  DSIN(YWRX2) 

MAT2T(1 ,3)  =  -DSIN(YWRX2) 

MAT2T(2,3)  =  DTAN(PTRY2)  *  DCOS(YWRX2) 

MAT2T(3,3)  =  1 .ODO/DCOS(PTRY2)  *  DCOS(YWRX2) 

GET  THE  YAW, PITCH  AND  THE  ROLL  RATES  FOR  LINK  TWO 

DO  707  J  =  1,3 
SUM1  =  O.ODO 
DO  708  K  *  1,3 

SUM1  =  SUM1  +  MAT2T(J,K)  *  BRATE2(K) 

708  CONTINUE 

RATE2 ( J)  =  SUM1 
707  CONTINUE 

RATE2X  =  RATE2 ( 1 ) 

RATE 2 Y  =  RATE2 (2 ) 

RATE2Z  =  RATE2(3) 

USE  THE  NEXT  THREE  STATEMENTS  FOR  CASE  A 

YWRX2  =  INTGRL(0 . .RATE2X) 

PTRY2  =  INTGRL ( 0 . , RATE2Y ) 

RLRZ2  =  INTGRL(-PI/2. ,RATE2Z) 

USE  THE  NEXT  THREE  STATEMENTS  FOR  CASE  B 

YWRX2  =  INTGRL ( 0. ,RATE2X) 

PTRY2  =  INTGRL ( 0. ,RATE2Y) 

RLRZ2  =  INTGRL(PI/2. ,RATE2Z) 

USE  THE  NEXT  THREE  STATEMENTS  FOR  CASE  C 

YWRX2  =  INTGRL (0 . , RATE2X) 

PTRY2  =  INTGRL ( 0. ,RATE2Y 
RLRZ2  =  INTGRL { 0. , RATE 2Z) 

RATE2(1)  =  RATE2X 
RATE2(2)  =  RATE2Y 
RATE2(3)  =  RATE 2 Z 

YAWANX(2)  =  YWRX2  *  RADEG 
PTCANY(2)  =  PTRY2  *  RADEG 
ROLANZ(2)  =  RLRZ2  *  RADEG 

USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  TWO  FOR  CASE  A 

DRCSY(2)  =  DCOS (RLRZ2 )  *  DSIN(PTRY2)  *  DSIN(YWRX2)  ”... 
DSIN(RLRZ2)  *  DCOS(YWRX2) 

DRCSX(2)  *  DSIN(RLRZ2)  *  DSIN(PTRY2)  *  DSIN(YWRX2)  +... 
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*  *  *  -K  •«  -K  *  *  *  *  *  *  ■*  *  *  *  *  VO  *  -It 


DC0S(RLRZ2)  *  DCOS(YWRX2) 

DRCS2(2)  -  DCOS(PTRY2)  *  DSIN(YWRX2) 

USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  TWO  FOR  CASE  B 

DRCSY(2)  =  DCOS(RLRZ2)  *  DCOS(PTRY2) 

DRCSX(2)  »  DSIN(RLR22)*DCOS(PTRY2) 


DRCSZ(2)  *  -DSIN{PTRY2) 

USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  TWO  FOR  CASE 

DRCSY(2)  =  DCOS(RLRZ2)  *  DSIN(PTRY2)  *  DCOS(YWRX2)  +... 
DSIN(RLRZ2)  *  DSIN(YWRX2) 

DRCSX(2)  =  DSIN(RLRZ2)  *  DSIN(PTRY2)  *  DCOS(YWRX2) 

DCOS(RLRZ2)  *  DSIN(YWRX2) 

DRCSZ<2)  =  DCOS(PTRY2)  *  DCOS(YWRX2) 

GET  THE  ANGLES  AS  RADIANS 

DRCRAX ( 2 )  =  DACOS(DRCSX(2) ) 

DRCRAY(2)  =  DACOS ( DRCS Y ( 2 ) ) 

DRCRAZ(2)  =  DAC0S(DRCSZ(2)) 

CONVERT  THE  DIRECTION  COSINES  TO  DEGREES 

DRCANX (2)  *  DACOS (DRCSX(2))  *  RADEG 
DRCANY (2)  =  DACOS(DRCSY(2))  *  RADEG 
DRCANZ ( 2 )  =  DACOS (DRCSZ(2))  *  RADEG 

FIND  THE  JOINT  LOCATION 


JX1  = 
JY1  = 
JZ1  = 

LINK  THREE 


L(l,l 
L(1 , 1 
L(l.l 


+  L(l,2 
+  L(1 ,2 
+  L(1 , 2 


*  DCOS ( DRCRAX ( 1 )  ) 

*  DCOS  ( DRCRAY ( 1 ) ) 

*  DCOS ( DRCRAZ ( 1 ) ) 


C 


AX3 

VELX3 

LCOGX3 

LCOGX(3) 


=  DQ(22) 

=  INTGRL (0 . , AX3) 

=  INTGRL ( X3 , VELX3 
*  LCOGX3 


AY3  *  DO (23) 

VELY3  =  INTGRL ( 0. ,AY3) 

LCOGY3  =  INTGRL  <  Y3 , VELY3 ) 

LCOGY ( 3 )  »  LCOGY3 


AZ3  =  DO ( 24 ) 

VELZ3  =  INTGRL (0.,AZ3) 

LC0GZ3  =  INTGRL (Z3,VELZ3) 

LCOGZ(3)  =  LCOGZ3 


WD3X  *  DQ(25) 

W3X  =  INTGRL (0.,WD3X) 

WDX(3)  =  WD3X 

W3(l)  ■  W3X 


WD3Y  *  DQ(26) 

W3Y  *  INTGRL ( 0. ,WD3Y) 

WDY(3)  =  WD3Y 

W3(2)  *  W3Y 


WD3Z 

W3Z 


WDZ ( 3 ) 
W3(3) 


*  I$^Gr2  ( 0 . ,  WD3Z  > 

*  WD3Z 
=  W3Z 


TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COOR.  SYSTEM 
FOR  THE  LINK  THREE 


MAT3R(1,1)  *  DCOS(RLRZ3 )  *  DCOS(PTRY3) 

MAT3R(2 , 1)  *  DC0S(RLRZ3)  *  DSIN(PTRY3)  *  DSIN(YWRX3)  "... 
DSIN(RLRZ3)  *  DCOS(YWRX3) 

MAT3R(3 , 1)  *  DCOS(RLRZ3)  *  DSIN(PTRY3)  *  DCOS(YWRX3)  +... 
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*  * 


DSIN(RLRZ3)  *  DSIN(YWRX3) 

MAT3R(1 ,2)  «  DSIN(RLRZ3)  *  DC0S(PTRY3) 

MAT3R(2,2)  *  DSIN(RLRZ3)  *  DSIN(PTRY3)  *  DSIN(YWRX3)  +... 
DC0S(RLRZ3)  *  DC0S(YWRX3) 

MAT3R(3,2)  *  DSIN(RLRZ3)  *  DSIN(PTRY3)  *  DC0S(YWRX3) 
DC0S(RLRZ3)  *  DSIN(YWRX3) 

MAT3R(1,3)  =  -DSIN(PTRY3) 

MAT3R(2,3)  =  DC0S(PTRY3)  *  DSIN(YWRX3) 

MAT3R(3,3)  =  DC0S(PTRY3)  *  DC0S(YWRX3) 

GET  THE  VELOCITIES  FOR  LINK  3  IN  BODY  FIXED  COOR.  SYSTEM 

DO  609  J  =  1,3 
SUM1  =  0.0D0 
DO  610  K  =  1,3 

SUM1  =  SUM1  +  MAT3R(J,K)  *  W3(K) 

610  CONTINUE 

BRATE3 ( J)  =  SUM1 
609  CONTINUE 

TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  EULER  COOR.  SYSTEM 
FOR  THE  LINK  THREE 

MAT3T(1 , 1 )  =  O.ODO 
MAT3T(2,1)  =  l.ODO 
MAT3T(3,1)  =  O.ODO 

MAT3T(1 ,2)  =  DCOS (YWRX3 ) 

MAT3T (2,2)  *  DTAN(PTRY3)  *  DSIN(YWRX3) 

MAT3T(3,2)  =  1 .ODO/DCOS (PTRY3)  *  DSIN(YWRX3) 

MAT3T(1 , 3 )  =  -DSIN(YWRX3) 

MAT3T (2,3)  =  DTAN(PTRY3)  *  DC0S(YWRX3) 

MAT3T(3,3)  =  1. ODO/DCOS (PTRY3)  *  DC0S(YWRX3) 

GET  THE  YAW, PITCH  AND  THE  ROLL  RATES  FOR  LINK  THREE 

DO  709  J  =  1,3 
SUM1  =  O.ODO 
DO  710  K  =  1,3 

SUM1  =  SUM1  +  MAT3T( J ,K)  *  BRATE3(K) 

710  CONTINUE 

RATE3( J)  =  SUM1 
709  CONTINUE 

RATE3X  =  RATE3(1) 

RATE 3 Y  =  RATE3 ( 2 ) 

RATE3Z  *  RATE3(3) 

USE  THE  NEXT  THREE  FOR  THE  CASE  A 

YWRX3  =  INTGRL(0. ,RATE3X) 

PTRY3  *  INTGRLiO. ,RATE3Y) 

RLRZ3  *  INTGRL(-PI/2. ,RATE3Z) 

USE  THE  NEXT  THREE  FOR  THE  CASE  B 

YWRX3  =  INTGRL(0. ,RATE3X) 

PTRY3  *  INTGRL ( 0 . , RATE3Y ) 

RLRZ3  =  INTGRL (PI/2., RATE3Z) 

USE  THE  NEXT  THREE  FOR  THE  CASE  C 

YWRX3  *  INTGRL(0.,RATE3X) 

PTRY3  *  INTGRL ( 0. ,RATE3Y) 

RLRZ3  =  INTGRL ( 0. , RATE 3Z) 

YAWANX ( 3 )  =  YWRX3  *  RADEG 
PTCANY(3)  *  PTRY3  *  RADEG 
ROLANZ ( 3 )  =  RLRZ3  *  RADEG 

USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  THREE  FOR  CASE  A 

DRCSY(3)  =  DCOS(RLRZ3)  *  DSIN(PTRY3)  *  DSIN(YWRX3)  -... 
DSIN(RLRZ3)  *  DCOS(YWRX3) 
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*  *  *  *  *  »»  *  *  *  ***  *  a- » 


•k 


DRCSX(3)  -  DSIN(RLRZ3)  *  DSIN(PTRY3)  *  DSIN(YWRX3)+. . . 
DC0S(RLRZ3)  *  DC0S(YWRX3) 

DRCSZ(3)  *  DC0S(PTRY3)  *  DSIN(YWRX3) 

USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  THREE  FOR  CASE  B 
DRCSY(3)  *  DCOS(RLRZ3)  *  DCOS(PTRY3) 

DRCSX(3)  «  DSIN(RLRZ3 ) *DCOS (PTRY3 ) 


DRCSZ(3)  *  -DSIN(PTRY3) 

USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  THREE  FOR  CASE 

DRCSY(3)  *  DCOS(RLRZ3)  *  DSIN(PTRY3)  *  DC0S(YWRX3)+. . . 
DSIN(RLRZ3)  *  DSIN(YWRX3) 

DRCSX(3)  =  DSIN(RLRZ3)  *  DSIN(PTRY3)  *  DCOS (YWRX3 ) - . . . 
DCOS(RLRZ3)  *  DSIN(YWRX3) 

DRCSZ(3)  =  DC0S(PTRY3)  *  DCOS(YWRX3) 

GET  THE  ANGLES  AS  RADIANS 

DRCRAX(3)  =  DACOS ( DRCSX ( 3 ) ) 

DRCRAY ( 3 )  =  DACOS ( DRCSY ( 3 ) ) 

DRCRAZ (3)  *  DACOS { DRCSZ ( 3 ) ) 

CONVERT  THE  DIRECTION  COSINES  TO  DEGREES 


DRCANX(3) 

DRCANY(3) 

DRCANZ(3) 


=  DACOS (DRCSX(3))  *  RADEG 
=  DACOS (DRCSY(3))  *  RADEG 
=  DACOS (DRCSZ(3))  *  RADEG 


FIND  ANGLE  BETWEEN  LINK  2  AND  LINK  3  TO  CHECK  IF  THE  ARM  LINKS 
are  PASSING  THROUGH  THE  SINGULAR  POINTS 


ANG23X  =  DRCANX(2)  -  DRCANX(3) 
ANG23Y  =  DRCANY(2)  -  DRCANY(3) 
ANG23Z  =  DRCANZ ( 2 )  -  DRCANZ(3) 


ANG12X  =  DRCANX(l)  -  DRCANX(2) 
ANG12Y  =  DRCANY(l)  -  DRCANY ( 2 ) 
ANG12Z  =  DRCANZ ( 1 )  -  DRCANZ (2) 


C 


FIND  THE  JOINT  LOCATION 

JX2  =  JX1  +  (L(2, 1) 
JY2  =  JY1  +  (L(2,l) 
JZ2  =  JZ1  +  (L(2, 1) 


+  L( 
+  L( 
+  L( 


*  DCOS ( DRCRAX ( 2 ) ) 

*  DCOS ( DRCRAY ( 2 ) ) 

*  DCOS ( DRCRAZ ( 2 ) ) 


TIPX  =  JX2  +  (L(3,l)  +  M3.2))  *  DCOS 
TIPY  =  JY2  +  (L(3 , 1 )  +  L(3 ,2) )  *  DCOS 
TIPZ  =  JZ2  +  (L(3,l)  +  L(3 ,2; )  *  DCOS 


(DRCRAX(3) ) 
(DRCRAY{3)) 
(DRCRAZ (3)) 


END 

STOP 

FORTRAN 

*  SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 


SUBROUTINE  CPROD ( VECTA , VECTB , MI , M J , MK) 
IMPLICIT  REAL*8  (A-Z) 

DIMENSION  VECTA(3) ,VECTB(3) 

MI  *  VECTA(2 )  *  VECTB (3)  -  VECTA( 
MJ  *  VECTA(3)  *  VECTB(l)  -  VECTA( 
MK  «  VECTA(l)  *  VECTB(2)  -  VECTA( 
RETURN 


*  VECTB(2) 

*  VECTB (3) 

*  VECTB ( 1 ) 


END 
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